From 2557f0d2de80e2df690b40b60f8ec79de799657e Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sat, 6 Apr 2013 23:04:32 -0700
Subject: Rename the 'device' directory back to 'drivers', it's less confusing
 that way.

Move the fmuv2 board driver out into the new world.
---
 src/drivers/lsm303d/lsm303d.cpp | 1290 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 1290 insertions(+)
 create mode 100644 src/drivers/lsm303d/lsm303d.cpp

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
new file mode 100644
index 000000000..32030a1f7
--- /dev/null
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -0,0 +1,1290 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file lsm303d.cpp
+ * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+
+#include <drivers/device/spi.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+/* SPI protocol address bits */
+#define DIR_READ				(1<<7)
+#define DIR_WRITE				(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+
+
+/* register addresses: A: accel, M: mag, T: temp */
+#define ADDR_WHO_AM_I			0x0F
+#define WHO_I_AM				0x49
+
+#define ADDR_OUT_L_T         	0x05
+#define ADDR_OUT_H_T        	0x06
+#define ADDR_STATUS_M           0x07
+#define ADDR_OUT_X_L_M          0x08
+#define ADDR_OUT_X_H_M          0x09
+#define ADDR_OUT_Y_L_M          0x0A
+#define ADDR_OUT_Y_H_M          0x0B
+#define ADDR_OUT_Z_L_M          0x0C
+#define ADDR_OUT_Z_H_M          0x0D
+
+#define ADDR_OUT_TEMP_A			0x26
+#define ADDR_STATUS_A			0x27
+#define ADDR_OUT_X_L_A			0x28
+#define ADDR_OUT_X_H_A			0x29
+#define ADDR_OUT_Y_L_A			0x2A
+#define ADDR_OUT_Y_H_A			0x2B
+#define ADDR_OUT_Z_L_A			0x2C
+#define ADDR_OUT_Z_H_A			0x2D
+
+#define ADDR_CTRL_REG0			0x1F
+#define ADDR_CTRL_REG1			0x20
+#define ADDR_CTRL_REG2			0x21
+#define ADDR_CTRL_REG3			0x22
+#define ADDR_CTRL_REG4			0x23
+#define ADDR_CTRL_REG5			0x24
+#define ADDR_CTRL_REG7			0x26
+
+#define REG1_POWERDOWN_A		((0<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_3_125HZ_A		((0<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_6_25HZ_A		((0<<7) | (0<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_12_5HZ_A		((0<<7) | (0<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_25HZ_A		((0<<7) | (1<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_50HZ_A		((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_100HZ_A		((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define REG1_RATE_200HZ_A		((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define REG1_RATE_400HZ_A		((1<<7) | (0<<6) | (0<<5) | (0<<4))
+#define REG1_RATE_800HZ_A		((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define REG1_RATE_1600HZ_A		((1<<7) | (0<<6) | (1<<5) | (0<<4))
+
+#define REG1_CONT_UPDATE_A		(0<<3)
+#define REG1_Z_ENABLE_A			(1<<2)
+#define REG1_Y_ENABLE_A			(1<<1)
+#define REG1_X_ENABLE_A			(1<<0)
+
+#define REG2_AA_FILTER_BW_773HZ_A		((0<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_194HZ_A		((0<<7) | (1<<6))
+#define REG2_AA_FILTER_BW_362HZ_A		((1<<7) | (0<<6))
+#define REG2_AA_FILTER_BW_50HZ_A		((1<<7) | (1<<6))
+
+#define REG2_FULL_SCALE_2G_A	((0<<5) | (0<<4) | (0<<3))
+#define REG2_FULL_SCALE_4G_A	((0<<5) | (0<<4) | (1<<3))
+#define REG2_FULL_SCALE_6G_A	((0<<5) | (1<<4) | (0<<3))
+#define REG2_FULL_SCALE_8G_A	((0<<5) | (1<<4) | (1<<3))
+#define REG2_FULL_SCALE_16G_A	((1<<5) | (0<<4) | (0<<3))
+
+#define REG5_ENABLE_T			(1<<7)
+
+#define REG5_RES_HIGH_M			((1<<6) | (1<<5))
+#define REG5_RES_LOW_M			((0<<6) | (0<<5))
+
+#define REG5_RATE_3_125HZ_M		((0<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_6_25HZ_M		((0<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_12_5HZ_M		((0<<4) | (1<<3) | (0<<2))
+#define REG5_RATE_25HZ_M		((0<<4) | (1<<3) | (1<<2))
+#define REG5_RATE_50HZ_M		((1<<4) | (0<<3) | (0<<2))
+#define REG5_RATE_100HZ_M		((1<<4) | (0<<3) | (1<<2))
+#define REG5_RATE_DO_NOT_USE_M	((1<<4) | (1<<3) | (0<<2))
+
+#define REG6_FULL_SCALE_2GA_M	((0<<7) | (0<<6))
+#define REG6_FULL_SCALE_4GA_M	((0<<7) | (1<<6))
+#define REG6_FULL_SCALE_8GA_M	((1<<7) | (0<<6))
+#define REG6_FULL_SCALE_12GA_M	((1<<7) | (1<<6))
+
+#define REG7_CONT_MODE_M		((0<<1) | (0<<0))
+
+
+#define INT_CTRL_M              0x12
+#define INT_SRC_M               0x13
+
+
+extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
+
+
+class LSM303D_mag;
+
+class LSM303D : public device::SPI
+{
+public:
+	LSM303D(int bus, const char* path, spi_dev_e device);
+	virtual ~LSM303D();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+	friend class LSM303D_mag;
+
+	virtual ssize_t		mag_read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+private:
+
+	LSM303D_mag		*_mag;
+
+	struct hrt_call		_accel_call;
+	struct hrt_call		_mag_call;
+
+	unsigned		_call_accel_interval;
+	unsigned		_call_mag_interval;
+
+	unsigned		_num_accel_reports;
+	volatile unsigned	_next_accel_report;
+	volatile unsigned	_oldest_accel_report;
+	struct accel_report	*_accel_reports;
+
+	struct accel_scale	_accel_scale;
+	float			_accel_range_scale;
+	float			_accel_range_m_s2;
+	orb_advert_t		_accel_topic;
+
+	unsigned		_num_mag_reports;
+	volatile unsigned	_next_mag_report;
+	volatile unsigned	_oldest_mag_report;
+	struct mag_report	*_mag_reports;
+
+	struct mag_scale	_mag_scale;
+	float			_mag_range_scale;
+	float			_mag_range_ga;
+	orb_advert_t		_mag_topic;
+
+	unsigned		_current_accel_rate;
+	unsigned		_current_accel_range;
+
+	unsigned		_current_mag_rate;
+	unsigned		_current_mag_range;
+
+	perf_counter_t		_accel_sample_perf;
+	perf_counter_t		_mag_sample_perf;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Static trampoline for the mag because it runs at a lower rate
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		mag_measure_trampoline(void *arg);
+
+	/**
+	 * Fetch accel measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Fetch mag measurements from the sensor and update the report ring.
+	 */
+	void			mag_measure();
+
+	/**
+	 * Read a register from the LSM303D
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the LSM303D
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the LSM303D
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the LSM303D measurement range.
+	 *
+	 * @param max_dps	The measurement range is set to permit reading at least
+	 *			this rate in degrees per second.
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			set_range(unsigned max_dps);
+
+	/**
+	 * Set the LSM303D internal sampling frequency.
+	 *
+	 * @param frequency	The internal sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int			set_samplerate(unsigned frequency);
+};
+
+/**
+ * Helper class implementing the mag driver node.
+ */
+class LSM303D_mag : public device::CDev
+{
+public:
+	LSM303D_mag(LSM303D *parent);
+	~LSM303D_mag();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+protected:
+	friend class LSM303D;
+
+	void			parent_poll_notify();
+private:
+	LSM303D			*_parent;
+
+	void			measure();
+
+	void			measure_trampoline(void *arg);
+};
+
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+
+LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
+	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
+	_mag(new LSM303D_mag(this)),
+	_call_accel_interval(0),
+	_call_mag_interval(0),
+	_num_accel_reports(0),
+	_next_accel_report(0),
+	_oldest_accel_report(0),
+	_accel_reports(nullptr),
+	_accel_range_scale(0.0f),
+	_accel_range_m_s2(0.0f),
+	_accel_topic(-1),
+	_num_mag_reports(0),
+	_next_mag_report(0),
+	_oldest_mag_report(0),
+	_mag_reports(nullptr),
+	_mag_range_scale(0.0f),
+	_mag_range_ga(0.0f),
+	_current_accel_rate(0),
+	_current_accel_range(0),
+	_current_mag_rate(0),
+	_current_mag_range(0),
+	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	/* XXX fix this default values */
+	_accel_range_scale = 1.0f;
+	_mag_range_scale = 1.0f;
+
+	// default scale factors
+	_accel_scale.x_offset = 0;
+	_accel_scale.x_scale  = 1.0f;
+	_accel_scale.y_offset = 0;
+	_accel_scale.y_scale  = 1.0f;
+	_accel_scale.z_offset = 0;
+	_accel_scale.z_scale  = 1.0f;
+
+	_mag_scale.x_offset = 0;
+	_mag_scale.x_scale = 1.0f;
+	_mag_scale.y_offset = 0;
+	_mag_scale.y_scale = 1.0f;
+	_mag_scale.z_offset = 0;
+	_mag_scale.z_scale = 1.0f;
+}
+
+LSM303D::~LSM303D()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_accel_reports != nullptr)
+		delete[] _accel_reports;
+	if (_mag_reports != nullptr)
+		delete[] _mag_reports;
+
+	delete _mag;
+
+	/* delete the perf counter */
+	perf_free(_accel_sample_perf);
+	perf_free(_mag_sample_perf);
+}
+
+int
+LSM303D::init()
+{
+	int ret = ERROR;
+	int mag_ret;
+	int fd_mag;
+
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_accel_reports = 2;
+	_oldest_accel_report = _next_accel_report = 0;
+	_accel_reports = new struct accel_report[_num_accel_reports];
+
+	if (_accel_reports == nullptr)
+		goto out;
+
+	/* advertise accel topic */
+	memset(&_accel_reports[0], 0, sizeof(_accel_reports[0]));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]);
+
+	_num_mag_reports = 2;
+	_oldest_mag_report = _next_mag_report = 0;
+	_mag_reports = new struct mag_report[_num_mag_reports];
+
+	if (_mag_reports == nullptr)
+		goto out;
+
+	/* advertise mag topic */
+	memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
+
+	/* XXX do this with ioctls */
+	/* set default configuration */
+	write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
+	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+	write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M);
+
+	/* XXX should we enable FIFO */
+
+	set_range(500);				/* default to 500dps */
+	set_samplerate(0);			/* max sample rate */
+
+//	_current_accel_rate = 100;
+
+	/* XXX test this when another mag is used */
+	/* do CDev init for the mag device node, keep it optional */
+	mag_ret = _mag->init();
+
+	if (mag_ret != OK) {
+		_mag_topic = -1;
+	}
+
+	ret = OK;
+out:
+	return ret;
+}
+
+int
+LSM303D::probe()
+{
+	/* read dummy value to void to clear SPI statemachine on sensor */
+	(void)read_reg(ADDR_WHO_AM_I);
+
+	/* verify that the device is attached and functioning */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+		return OK;
+
+	return -EIO;
+}
+
+ssize_t
+LSM303D::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct accel_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_accel_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the measurement code while we are doing this;
+		 * we are careful to avoid racing with it.
+		 */
+		while (count--) {
+			if (_oldest_accel_report != _next_accel_report) {
+				memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports));
+				ret += sizeof(_accel_reports[0]);
+				INCREMENT(_oldest_accel_report, _num_accel_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_accel_report = _next_accel_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _accel_reports, sizeof(*_accel_reports));
+	ret = sizeof(*_accel_reports);
+
+	return ret;
+}
+
+ssize_t
+LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct mag_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_mag_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the measurement code while we are doing this;
+		 * we are careful to avoid racing with it.
+		 */
+		while (count--) {
+			if (_oldest_mag_report != _next_mag_report) {
+				memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports));
+				ret += sizeof(_mag_reports[0]);
+				INCREMENT(_oldest_mag_report, _num_mag_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_mag_report = _next_mag_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _mag_reports, sizeof(*_mag_reports));
+	ret = sizeof(*_mag_reports);
+
+	return ret;
+}
+
+int
+LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_accel_interval = 0;
+				return OK;
+
+				/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT:
+				/* With internal low pass filters enabled, 250 Hz is sufficient */
+				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_accel_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_accel_call.period = _call_accel_interval = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_accel_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_accel_interval;
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* account for sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct accel_report *buf = new struct accel_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _accel_reports;
+			_num_accel_reports = arg;
+			_accel_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_accel_reports - 1;
+
+	case SENSORIOCRESET:
+		/* XXX implement */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+int
+LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+		switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_call_mag_interval = 0;
+				return OK;
+
+			/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT:
+				/* 50 Hz is max for mag */
+				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50);
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_mag_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_mag_call.period = _call_mag_interval = ticks;
+
+
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_call_mag_interval == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return 1000000 / _call_mag_interval;
+	case SENSORIOCSQUEUEDEPTH:
+	case SENSORIOCGQUEUEDEPTH:
+	case SENSORIOCRESET:
+		return ioctl(filp, cmd, arg);
+
+	case MAGIOCSSAMPLERATE:
+//	case MAGIOCGSAMPLERATE:
+		/* XXX not implemented */
+		return -EINVAL;
+
+	case MAGIOCSLOWPASS:
+//	case MAGIOCGLOWPASS:
+		/* XXX not implemented */
+//		_set_dlpf_filter((uint16_t)arg);
+		return -EINVAL;
+
+	case MAGIOCSSCALE:
+		/* copy scale in */
+		memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
+		return OK;
+
+	case MAGIOCSRANGE:
+//	case MAGIOCGRANGE:
+		/* XXX not implemented */
+		// XXX change these two values on set:
+		// _mag_range_scale = xx
+		// _mag_range_ga = xx
+		return -EINVAL;
+
+	case MAGIOCSELFTEST:
+		/* XXX not implemented */
+//		return self_test();
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+uint8_t
+LSM303D::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+LSM303D::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_reg(reg, val);
+}
+
+int
+LSM303D::set_range(unsigned max_dps)
+{
+	/* XXX implement this */
+//	uint8_t bits = REG4_BDU;
+//
+//	if (max_dps == 0)
+//		max_dps = 2000;
+//
+//	if (max_dps <= 250) {
+//		_current_range = 250;
+//		bits |= RANGE_250DPS;
+//
+//	} else if (max_dps <= 500) {
+//		_current_range = 500;
+//		bits |= RANGE_500DPS;
+//
+//	} else if (max_dps <= 2000) {
+//		_current_range = 2000;
+//		bits |= RANGE_2000DPS;
+//
+//	} else {
+//		return -EINVAL;
+//	}
+//
+//	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
+//	_gyro_range_scale = _gyro_range_rad_s / 32768.0f;
+//	write_reg(ADDR_CTRL_REG4, bits);
+
+	return OK;
+}
+
+int
+LSM303D::set_samplerate(unsigned frequency)
+{
+	/* XXX implement this */
+//	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
+//
+//	if (frequency == 0)
+//		frequency = 760;
+//
+//	if (frequency <= 95) {
+//		_current_rate = 95;
+//		bits |= RATE_95HZ_LP_25HZ;
+//
+//	} else if (frequency <= 190) {
+//		_current_rate = 190;
+//		bits |= RATE_190HZ_LP_25HZ;
+//
+//	} else if (frequency <= 380) {
+//		_current_rate = 380;
+//		bits |= RATE_380HZ_LP_30HZ;
+//
+//	} else if (frequency <= 760) {
+//		_current_rate = 760;
+//		bits |= RATE_760HZ_LP_30HZ;
+//
+//	} else {
+//		return -EINVAL;
+//	}
+//
+//	write_reg(ADDR_CTRL_REG1, bits);
+
+	return OK;
+}
+
+void
+LSM303D::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring */
+	_oldest_accel_report = _next_accel_report = 0;
+	_oldest_mag_report = _next_mag_report = 0;
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
+	hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
+}
+
+void
+LSM303D::stop()
+{
+	hrt_cancel(&_accel_call);
+	hrt_cancel(&_mag_call);
+}
+
+void
+LSM303D::measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+LSM303D::mag_measure_trampoline(void *arg)
+{
+	LSM303D *dev = (LSM303D *)arg;
+
+	/* make another measurement */
+	dev->mag_measure();
+}
+
+void
+LSM303D::measure()
+{
+	/* status register and data as read back from the device */
+
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_accel_report;
+#pragma pack(pop)
+
+	accel_report		*accel_report = &_accel_reports[_next_accel_report];
+
+	/* start the performance counter */
+	perf_begin(_accel_sample_perf);
+
+	/* fetch data from the sensor */
+	raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
+
+	/* XXX adapt the comment to specs */
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+
+	accel_report->timestamp = hrt_absolute_time();
+	/* XXX adjust for sensor alignment to board here */
+	accel_report->x_raw = raw_accel_report.x;
+	accel_report->y_raw = raw_accel_report.y;
+	accel_report->z_raw = raw_accel_report.z;
+
+	accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+//	report->scaling = _gyro_range_scale;
+//	report->range_rad_s = _gyro_range_rad_s;
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_accel_report, _num_accel_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_accel_report == _oldest_accel_report)
+		INCREMENT(_oldest_accel_report, _num_accel_reports);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
+
+	/* stop the perf counter */
+	perf_end(_accel_sample_perf);
+}
+
+void
+LSM303D::mag_measure()
+{
+	/* status register and data as read back from the device */
+#pragma pack(push, 1)
+	struct {
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} raw_mag_report;
+#pragma pack(pop)
+
+	mag_report		*mag_report = &_mag_reports[_next_mag_report];
+
+	/* start the performance counter */
+	perf_begin(_mag_sample_perf);
+
+	/* fetch data from the sensor */
+	raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
+	transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
+
+	/* XXX adapt the comment to specs */
+	/*
+	 * 1) Scale raw value to SI units using scaling from datasheet.
+	 * 2) Subtract static offset (in SI units)
+	 * 3) Scale the statically calibrated values with a linear
+	 *    dynamically obtained factor
+	 *
+	 * Note: the static sensor offset is the number the sensor outputs
+	 * 	 at a nominally 'zero' input. Therefore the offset has to
+	 * 	 be subtracted.
+	 *
+	 *	 Example: A gyro outputs a value of 74 at zero angular rate
+	 *	 	  the offset is 74 from the origin and subtracting
+	 *		  74 from all measurements centers them around zero.
+	 */
+
+
+	mag_report->timestamp = hrt_absolute_time();
+	/* XXX adjust for sensor alignment to board here */
+	mag_report->x_raw = raw_mag_report.x;
+	mag_report->y_raw = raw_mag_report.y;
+	mag_report->z_raw = raw_mag_report.z;
+	mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+	mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+	mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
+//	report->scaling = _gyro_range_scale;
+//	report->range_rad_s = _gyro_range_rad_s;
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_mag_report, _num_mag_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_mag_report == _oldest_mag_report)
+		INCREMENT(_oldest_mag_report, _num_mag_reports);
+
+	/* XXX please check this poll_notify, is it the right one? */
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	/* publish for subscribers */
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
+
+	/* stop the perf counter */
+	perf_end(_mag_sample_perf);
+}
+
+void
+LSM303D::print_info()
+{
+	perf_print_counter(_accel_sample_perf);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
+	perf_print_counter(_mag_sample_perf);
+		printf("report queue:   %u (%u/%u @ %p)\n",
+		   _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports);
+}
+
+LSM303D_mag::LSM303D_mag(LSM303D *parent) :
+	CDev("LSM303D_mag", MAG_DEVICE_PATH),
+	_parent(parent)
+{
+}
+
+LSM303D_mag::~LSM303D_mag()
+{
+}
+
+void
+LSM303D_mag::parent_poll_notify()
+{
+	poll_notify(POLLIN);
+}
+
+ssize_t
+LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return _parent->mag_read(filp, buffer, buflen);
+}
+
+int
+LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	return _parent->mag_ioctl(filp, cmd, arg);
+}
+
+void
+LSM303D_mag::measure()
+{
+	_parent->mag_measure();
+}
+
+void
+LSM303D_mag::measure_trampoline(void *arg)
+{
+	_parent->mag_measure_trampoline(arg);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace lsm303d
+{
+
+LSM303D	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd, fd_mag;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	/* don't fail if open cannot be opened */
+	if (0 <= fd_mag) {
+		if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+			goto fail;
+		}
+	}
+
+
+	exit(0);
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	int fd_accel = -1;
+	struct accel_report a_report;
+	ssize_t sz;
+
+	/* get the driver */
+	fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd_accel < 0)
+		err(1, "%s open failed", ACCEL_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd_accel, &a_report, sizeof(a_report));
+
+	if (sz != sizeof(a_report))
+		err(1, "immediate read failed");
+
+	/* XXX fix the test output */
+//	warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
+//	warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
+//	warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
+	warnx("accel x: \t%d\traw", (int)a_report.x_raw);
+	warnx("accel y: \t%d\traw", (int)a_report.y_raw);
+	warnx("accel z: \t%d\traw", (int)a_report.z_raw);
+//	warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
+
+
+
+	int fd_mag = -1;
+	struct mag_report m_report;
+
+	/* get the driver */
+	fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd_mag < 0)
+		err(1, "%s open failed", MAG_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd_mag, &m_report, sizeof(m_report));
+
+	if (sz != sizeof(m_report))
+		err(1, "immediate read failed");
+
+	/* XXX fix the test output */
+	warnx("mag x: \t%d\traw", (int)m_report.x_raw);
+	warnx("mag y: \t%d\traw", (int)m_report.y_raw);
+	warnx("mag z: \t%d\traw", (int)m_report.z_raw);
+
+	/* XXX add poll-rate tests here too */
+
+//	reset();
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running\n");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+
+} // namespace
+
+int
+lsm303d_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(argv[1], "start"))
+		lsm303d::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		lsm303d::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		lsm303d::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		lsm303d::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}
-- 
cgit v1.2.3


From eb3d6f228cd96ef2a2b8a33f667dd1cdc2d5fdc5 Mon Sep 17 00:00:00 2001
From: Julian Oes <joes@student.ethz.ch>
Date: Mon, 8 Apr 2013 21:53:23 -0700
Subject: Added some functions for changing rates etc (WIP)

---
 src/drivers/lsm303d/lsm303d.cpp | 377 +++++++++++++++++++++++++++-------------
 1 file changed, 257 insertions(+), 120 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 32030a1f7..ff56bff17 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -108,8 +108,10 @@ static const int ERROR = -1;
 #define ADDR_CTRL_REG3			0x22
 #define ADDR_CTRL_REG4			0x23
 #define ADDR_CTRL_REG5			0x24
+#define ADDR_CTRL_REG6			0x25
 #define ADDR_CTRL_REG7			0x26
 
+#define REG1_RATE_BITS_A		((1<<7) | (1<<6) | (1<<5) | (1<<4))
 #define REG1_POWERDOWN_A		((0<<7) | (0<<6) | (0<<5) | (0<<4))
 #define REG1_RATE_3_125HZ_A		((0<<7) | (0<<6) | (0<<5) | (1<<4))
 #define REG1_RATE_6_25HZ_A		((0<<7) | (0<<6) | (1<<5) | (0<<4))
@@ -127,11 +129,13 @@ static const int ERROR = -1;
 #define REG1_Y_ENABLE_A			(1<<1)
 #define REG1_X_ENABLE_A			(1<<0)
 
+#define REG2_ANTIALIAS_FILTER_BW_BITS_A	((1<<7) | (1<<6))
 #define REG2_AA_FILTER_BW_773HZ_A		((0<<7) | (0<<6))
 #define REG2_AA_FILTER_BW_194HZ_A		((0<<7) | (1<<6))
 #define REG2_AA_FILTER_BW_362HZ_A		((1<<7) | (0<<6))
 #define REG2_AA_FILTER_BW_50HZ_A		((1<<7) | (1<<6))
 
+#define REG2_FULL_SCALE_BITS_A	((1<<5) | (1<<4) | (1<<3))
 #define REG2_FULL_SCALE_2G_A	((0<<5) | (0<<4) | (0<<3))
 #define REG2_FULL_SCALE_4G_A	((0<<5) | (0<<4) | (1<<3))
 #define REG2_FULL_SCALE_6G_A	((0<<5) | (1<<4) | (0<<3))
@@ -143,6 +147,7 @@ static const int ERROR = -1;
 #define REG5_RES_HIGH_M			((1<<6) | (1<<5))
 #define REG5_RES_LOW_M			((0<<6) | (0<<5))
 
+#define REG5_RATE_BITS_M		((1<<4) | (1<<3) | (1<<2))
 #define REG5_RATE_3_125HZ_M		((0<<4) | (0<<3) | (0<<2))
 #define REG5_RATE_6_25HZ_M		((0<<4) | (0<<3) | (1<<2))
 #define REG5_RATE_12_5HZ_M		((0<<4) | (1<<3) | (0<<2))
@@ -151,6 +156,7 @@ static const int ERROR = -1;
 #define REG5_RATE_100HZ_M		((1<<4) | (0<<3) | (1<<2))
 #define REG5_RATE_DO_NOT_USE_M	((1<<4) | (1<<3) | (0<<2))
 
+#define REG6_FULL_SCALE_BITS_M	((1<<7) | (1<<6))
 #define REG6_FULL_SCALE_2GA_M	((0<<7) | (0<<6))
 #define REG6_FULL_SCALE_4GA_M	((0<<7) | (1<<6))
 #define REG6_FULL_SCALE_8GA_M	((1<<7) | (0<<6))
@@ -174,72 +180,66 @@ public:
 	LSM303D(int bus, const char* path, spi_dev_e device);
 	virtual ~LSM303D();
 
-	virtual int		init();
+	virtual int			init();
 
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
 
 	/**
 	 * Diagnostics - print some basic information about the driver.
 	 */
-	void			print_info();
+	void				print_info();
 
 protected:
-	virtual int		probe();
+	virtual int			probe();
 
-	friend class LSM303D_mag;
+	friend class 		LSM303D_mag;
 
 	virtual ssize_t		mag_read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int			mag_ioctl(struct file *filp, int cmd, unsigned long arg);
 
 private:
 
-	LSM303D_mag		*_mag;
+	LSM303D_mag			*_mag;
 
 	struct hrt_call		_accel_call;
 	struct hrt_call		_mag_call;
 
-	unsigned		_call_accel_interval;
-	unsigned		_call_mag_interval;
+	unsigned			_call_accel_interval;
+	unsigned			_call_mag_interval;
 
-	unsigned		_num_accel_reports;
+	unsigned			_num_accel_reports;
 	volatile unsigned	_next_accel_report;
 	volatile unsigned	_oldest_accel_report;
 	struct accel_report	*_accel_reports;
 
 	struct accel_scale	_accel_scale;
-	float			_accel_range_scale;
-	float			_accel_range_m_s2;
+	float				_accel_range_scale;
+	float				_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
 
-	unsigned		_num_mag_reports;
+	unsigned			_num_mag_reports;
 	volatile unsigned	_next_mag_report;
 	volatile unsigned	_oldest_mag_report;
 	struct mag_report	*_mag_reports;
 
 	struct mag_scale	_mag_scale;
-	float			_mag_range_scale;
-	float			_mag_range_ga;
+	float				_mag_range_scale;
+	float				_mag_range_ga;
 	orb_advert_t		_mag_topic;
 
-	unsigned		_current_accel_rate;
-	unsigned		_current_accel_range;
-
-	unsigned		_current_mag_rate;
-	unsigned		_current_mag_range;
-
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
 
 	/**
 	 * Start automatic measurement.
 	 */
-	void			start();
+	void				start();
 
 	/**
 	 * Stop automatic measurement.
 	 */
-	void			stop();
+	void				stop();
 
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
@@ -250,24 +250,24 @@ private:
 	 *
 	 * @param arg		Instance pointer for the driver that is polling.
 	 */
-	static void		measure_trampoline(void *arg);
+	static void			measure_trampoline(void *arg);
 
 	/**
 	 * Static trampoline for the mag because it runs at a lower rate
 	 *
 	 * @param arg		Instance pointer for the driver that is polling.
 	 */
-	static void		mag_measure_trampoline(void *arg);
+	static void			mag_measure_trampoline(void *arg);
 
 	/**
 	 * Fetch accel measurements from the sensor and update the report ring.
 	 */
-	void			measure();
+	void				measure();
 
 	/**
 	 * Fetch mag measurements from the sensor and update the report ring.
 	 */
-	void			mag_measure();
+	void				mag_measure();
 
 	/**
 	 * Read a register from the LSM303D
@@ -275,7 +275,7 @@ private:
 	 * @param		The register to read.
 	 * @return		The value that was read.
 	 */
-	uint8_t			read_reg(unsigned reg);
+	uint8_t				read_reg(unsigned reg);
 
 	/**
 	 * Write a register in the LSM303D
@@ -283,7 +283,7 @@ private:
 	 * @param reg		The register to write.
 	 * @param value		The new value to write.
 	 */
-	void			write_reg(unsigned reg, uint8_t value);
+	void				write_reg(unsigned reg, uint8_t value);
 
 	/**
 	 * Modify a register in the LSM303D
@@ -294,27 +294,54 @@ private:
 	 * @param clearbits	Bits in the register to clear.
 	 * @param setbits	Bits in the register to set.
 	 */
-	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+	void				modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the LSM303D accel measurement range.
+	 *
+	 * @param max_g	The measurement range of the accel is in g (9.81m/s^2)
+	 *			Zero selects the maximum supported range.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int					set_range(unsigned max_g);
 
 	/**
-	 * Set the LSM303D measurement range.
+	 * Set the LSM303D mag measurement range.
 	 *
-	 * @param max_dps	The measurement range is set to permit reading at least
-	 *			this rate in degrees per second.
+	 * @param max_ga	The measurement range of the mag is in Ga
 	 *			Zero selects the maximum supported range.
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int			set_range(unsigned max_dps);
+	int					mag_set_range(unsigned max_g);
+
+	/**
+	 * Set the LSM303D accel anti-alias filter.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int					set_antialias_filter_bandwidth(unsigned max_g);
+
+	/**
+	 * Set the LSM303D internal accel sampling frequency.
+	 *
+	 * @param frequency	The internal accel sampling frequency is set to not less than
+	 *			this value.
+	 *			Zero selects the maximum rate supported.
+	 * @return		OK if the value can be supported.
+	 */
+	int					set_samplerate(unsigned frequency);
 
 	/**
-	 * Set the LSM303D internal sampling frequency.
+	 * Set the LSM303D internal mag sampling frequency.
 	 *
-	 * @param frequency	The internal sampling frequency is set to not less than
+	 * @param frequency	The internal mag sampling frequency is set to not less than
 	 *			this value.
 	 *			Zero selects the maximum rate supported.
 	 * @return		OK if the value can be supported.
 	 */
-	int			set_samplerate(unsigned frequency);
+	int					mag_set_samplerate(unsigned frequency);
 };
 
 /**
@@ -327,18 +354,18 @@ public:
 	~LSM303D_mag();
 
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
 
 protected:
 	friend class LSM303D;
 
-	void			parent_poll_notify();
+	void				parent_poll_notify();
 private:
-	LSM303D			*_parent;
+	LSM303D				*_parent;
 
-	void			measure();
+	void				measure();
 
-	void			measure_trampoline(void *arg);
+	void				measure_trampoline(void *arg);
 };
 
 
@@ -364,19 +391,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_reports(nullptr),
 	_mag_range_scale(0.0f),
 	_mag_range_ga(0.0f),
-	_current_accel_rate(0),
-	_current_accel_range(0),
-	_current_mag_rate(0),
-	_current_mag_range(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read"))
 {
 	// enable debug() calls
 	_debug_enabled = true;
 
-	/* XXX fix this default values */
-	_accel_range_scale = 1.0f;
-	_mag_range_scale = 1.0f;
+	_mag_range_scale = 12.0f/32767.0f;
 
 	// default scale factors
 	_accel_scale.x_offset = 0;
@@ -446,18 +467,21 @@ LSM303D::init()
 	memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
 	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
 
-	/* XXX do this with ioctls */
-	/* set default configuration */
-	write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
+	/* enable accel, XXX do this with an ioctl? */
+	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
+
+	/* enable mag, XXX do this with an ioctl? */
 	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
-	write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M);
+	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
 
-	/* XXX should we enable FIFO */
+	/* XXX should we enable FIFO? */
 
-	set_range(500);				/* default to 500dps */
-	set_samplerate(0);			/* max sample rate */
+	set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */
+	set_antialias_filter_bandwidth(194); /* XXX: choose bandwidth: 50, 194, 362 or 773 Hz */
+	set_samplerate(400); /* max sample rate */
 
-//	_current_accel_rate = 100;
+	mag_set_range(12); /* XXX: take highest sensor range of 12GA? */
+	mag_set_samplerate(100);
 
 	/* XXX test this when another mag is used */
 	/* do CDev init for the mag device node, keep it optional */
@@ -590,9 +614,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
+
+				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
+
 			case SENSOR_POLLRATE_DEFAULT:
 				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+				/* XXX for vibration tests with 800 Hz */
+				return ioctl(filp, SENSORIOCSPOLLRATE, 800);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -606,6 +634,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 					if (ticks < 1000)
 						return -EINVAL;
 
+					/* adjust sample rate of sensor */
+					set_samplerate(arg);
+
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_accel_call.period = _call_accel_interval = ticks;
@@ -801,33 +832,111 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 }
 
 int
-LSM303D::set_range(unsigned max_dps)
+LSM303D::set_range(unsigned max_g)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
+	float new_range = 0.0f;
+
+	if (max_g == 0)
+		max_g = 16;
+
+	if (max_g <= 2) {
+		new_range = 2.0f;
+		setbits |= REG2_FULL_SCALE_2G_A;
+
+	} else if (max_g <= 4) {
+		new_range = 4.0f;
+		setbits |= REG2_FULL_SCALE_4G_A;
+
+	} else if (max_g <= 6) {
+		new_range = 6.0f;
+		setbits |= REG2_FULL_SCALE_6G_A;
+
+	} else if (max_g <= 8) {
+		new_range = 8.0f;
+		setbits |= REG2_FULL_SCALE_8G_A;
+
+	} else if (max_g <= 16) {
+		new_range = 16.0f;
+		setbits |= REG2_FULL_SCALE_16G_A;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_accel_range_m_s2 = new_range * 9.80665f;
+	_accel_range_scale = _accel_range_m_s2 / 32768.0f;
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_range(unsigned max_ga)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
+	float new_range = 0.0f;
+
+	if (max_ga == 0)
+		max_ga = 12;
+
+	if (max_ga <= 2) {
+		new_range = 2.0f;
+		setbits |= REG6_FULL_SCALE_2GA_M;
+
+	} else if (max_ga <= 4) {
+		new_range = 4.0f;
+		setbits |= REG6_FULL_SCALE_4GA_M;
+
+	} else if (max_ga <= 8) {
+		new_range = 8.0f;
+		setbits |= REG6_FULL_SCALE_8GA_M;
+
+	} else if (max_ga <= 12) {
+		new_range = 12.0f;
+		setbits |= REG6_FULL_SCALE_12GA_M;
+
+	} else {
+		return -EINVAL;
+	}
+
+	_mag_range_ga = new_range;
+	_mag_range_scale = _mag_range_ga / 32768.0f;
+
+	modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
 {
-	/* XXX implement this */
-//	uint8_t bits = REG4_BDU;
-//
-//	if (max_dps == 0)
-//		max_dps = 2000;
-//
-//	if (max_dps <= 250) {
-//		_current_range = 250;
-//		bits |= RANGE_250DPS;
-//
-//	} else if (max_dps <= 500) {
-//		_current_range = 500;
-//		bits |= RANGE_500DPS;
-//
-//	} else if (max_dps <= 2000) {
-//		_current_range = 2000;
-//		bits |= RANGE_2000DPS;
-//
-//	} else {
-//		return -EINVAL;
-//	}
-//
-//	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
-//	_gyro_range_scale = _gyro_range_rad_s / 32768.0f;
-//	write_reg(ADDR_CTRL_REG4, bits);
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
+
+	if (bandwidth == 0)
+		bandwidth = 773;
+
+	if (bandwidth <= 50) {
+		setbits |= REG2_AA_FILTER_BW_50HZ_A;
+
+	} else if (bandwidth <= 194) {
+		setbits |= REG2_AA_FILTER_BW_194HZ_A;
+
+	} else if (bandwidth <= 362) {
+		setbits |= REG2_AA_FILTER_BW_362HZ_A;
+
+	} else if (bandwidth <= 773) {
+		setbits |= REG2_AA_FILTER_BW_773HZ_A;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
 	return OK;
 }
@@ -835,33 +944,60 @@ LSM303D::set_range(unsigned max_dps)
 int
 LSM303D::set_samplerate(unsigned frequency)
 {
-	/* XXX implement this */
-//	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
-//
-//	if (frequency == 0)
-//		frequency = 760;
-//
-//	if (frequency <= 95) {
-//		_current_rate = 95;
-//		bits |= RATE_95HZ_LP_25HZ;
-//
-//	} else if (frequency <= 190) {
-//		_current_rate = 190;
-//		bits |= RATE_190HZ_LP_25HZ;
-//
-//	} else if (frequency <= 380) {
-//		_current_rate = 380;
-//		bits |= RATE_380HZ_LP_30HZ;
-//
-//	} else if (frequency <= 760) {
-//		_current_rate = 760;
-//		bits |= RATE_760HZ_LP_30HZ;
-//
-//	} else {
-//		return -EINVAL;
-//	}
-//
-//	write_reg(ADDR_CTRL_REG1, bits);
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG1_RATE_BITS_A;
+
+	if (frequency == 0)
+		frequency = 1600;
+
+	if (frequency <= 100) {
+		setbits |= REG1_RATE_100HZ_A;
+
+	} else if (frequency <= 200) {
+		setbits |= REG1_RATE_200HZ_A;
+
+	} else if (frequency <= 400) {
+		setbits |= REG1_RATE_400HZ_A;
+
+	} else if (frequency <= 800) {
+		setbits |= REG1_RATE_800HZ_A;
+
+	} else if (frequency <= 1600) {
+		setbits |= REG1_RATE_1600HZ_A;
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+
+	return OK;
+}
+
+int
+LSM303D::mag_set_samplerate(unsigned frequency)
+{
+	uint8_t setbits = 0;
+	uint8_t clearbits = REG5_RATE_BITS_M;
+
+	if (frequency == 0)
+		frequency = 100;
+
+	if (frequency <= 25) {
+		setbits |= REG5_RATE_25HZ_M;
+
+	} else if (frequency <= 50) {
+		setbits |= REG5_RATE_50HZ_M;
+
+	} else if (frequency <= 100) {
+		setbits |= REG5_RATE_100HZ_M;
+
+
+	} else {
+		return -EINVAL;
+	}
+
+	modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
 
 	return OK;
 }
@@ -956,8 +1092,8 @@ LSM303D::measure()
 	accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
 	accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
 	accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-//	report->scaling = _gyro_range_scale;
-//	report->range_rad_s = _gyro_range_rad_s;
+	accel_report->scaling = _accel_range_scale;
+	accel_report->range_m_s2 = _accel_range_m_s2;
 
 	/* post a report to the ring - note, not locked */
 	INCREMENT(_next_accel_report, _num_accel_reports);
@@ -1183,15 +1319,13 @@ test()
 	if (sz != sizeof(a_report))
 		err(1, "immediate read failed");
 
-	/* XXX fix the test output */
-//	warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
-//	warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
-//	warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
+	warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
+	warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
+	warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
 	warnx("accel x: \t%d\traw", (int)a_report.x_raw);
 	warnx("accel y: \t%d\traw", (int)a_report.y_raw);
 	warnx("accel z: \t%d\traw", (int)a_report.z_raw);
-//	warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
-
+	warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
 
 
 	int fd_mag = -1;
@@ -1209,10 +1343,13 @@ test()
 	if (sz != sizeof(m_report))
 		err(1, "immediate read failed");
 
-	/* XXX fix the test output */
+	warnx("mag x: \t% 9.5f\tga", (double)m_report.x);
+	warnx("mag y: \t% 9.5f\tga", (double)m_report.y);
+	warnx("mag z: \t% 9.5f\tga", (double)m_report.z);
 	warnx("mag x: \t%d\traw", (int)m_report.x_raw);
 	warnx("mag y: \t%d\traw", (int)m_report.y_raw);
 	warnx("mag z: \t%d\traw", (int)m_report.z_raw);
+	warnx("mag range: %8.4f ga", (double)m_report.range_ga);
 
 	/* XXX add poll-rate tests here too */
 
-- 
cgit v1.2.3


From 1d327c42a6f34f8545940cd8630586726e4d3ae6 Mon Sep 17 00:00:00 2001
From: Julian Oes <joes@student.ethz.ch>
Date: Mon, 8 Apr 2013 22:04:21 -0700
Subject: Mag sample rate was not actually changed by an ioctl

---
 src/drivers/lsm303d/lsm303d.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index ff56bff17..b107b869f 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -718,7 +718,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
 				/* 50 Hz is max for mag */
-				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50);
+				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
 
 			/* adjust to a legal polling interval in Hz */
 			default: {
@@ -732,12 +732,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 					if (ticks < 1000)
 						return -EINVAL;
 
+					/* adjust sample rate of sensor */
+					mag_set_samplerate(arg);
+
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_mag_call.period = _call_mag_interval = ticks;
 
-
-
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
 						start();
-- 
cgit v1.2.3


From 3469fefe117f8fa3bfef3fbcb3751a32e81c324a Mon Sep 17 00:00:00 2001
From: Julian Oes <joes@student.ethz.ch>
Date: Mon, 8 Apr 2013 23:47:19 -0700
Subject: Checked axes of LSM303D

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

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index b107b869f..8a64ee702 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1067,7 +1067,6 @@ LSM303D::measure()
 	raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
 	transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
 
-	/* XXX adapt the comment to specs */
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
 	 * 2) Subtract static offset (in SI units)
@@ -1085,7 +1084,7 @@ LSM303D::measure()
 
 
 	accel_report->timestamp = hrt_absolute_time();
-	/* XXX adjust for sensor alignment to board here */
+
 	accel_report->x_raw = raw_accel_report.x;
 	accel_report->y_raw = raw_accel_report.y;
 	accel_report->z_raw = raw_accel_report.z;
@@ -1136,7 +1135,6 @@ LSM303D::mag_measure()
 	raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
 	transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
 
-	/* XXX adapt the comment to specs */
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
 	 * 2) Subtract static offset (in SI units)
@@ -1154,15 +1152,15 @@ LSM303D::mag_measure()
 
 
 	mag_report->timestamp = hrt_absolute_time();
-	/* XXX adjust for sensor alignment to board here */
+
 	mag_report->x_raw = raw_mag_report.x;
 	mag_report->y_raw = raw_mag_report.y;
 	mag_report->z_raw = raw_mag_report.z;
 	mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
 	mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
 	mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
-//	report->scaling = _gyro_range_scale;
-//	report->range_rad_s = _gyro_range_rad_s;
+	mag_report->scaling = _mag_range_scale;
+	mag_report->range_ga = _mag_range_ga;
 
 	/* post a report to the ring - note, not locked */
 	INCREMENT(_next_mag_report, _num_mag_reports);
-- 
cgit v1.2.3


From b7e947cb3d53cbb0f9b194980a6a63588ba56bf2 Mon Sep 17 00:00:00 2001
From: Julian Oes <joes@student.ethz.ch>
Date: Wed, 17 Apr 2013 11:22:08 -0700
Subject: Anti-Aliasing frequency of the LSM303D can now be read too, not just
 written

---
 src/drivers/lsm303d/lsm303d.cpp | 52 +++++++++++++++++++++++++++++++++++++----
 1 file changed, 48 insertions(+), 4 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 8a64ee702..ba7316e55 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -321,7 +321,15 @@ private:
 	 * 			Zero selects the highest bandwidth
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int					set_antialias_filter_bandwidth(unsigned max_g);
+	int					set_antialias_filter_bandwidth(unsigned bandwith);
+
+	/**
+	 * Get the LSM303D accel anti-alias filter.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * @return		OK if the value was read and supported, ERROR otherwise.
+	 */
+	int					get_antialias_filter_bandwidth(unsigned &bandwidth);
 
 	/**
 	 * Set the LSM303D internal accel sampling frequency.
@@ -477,10 +485,10 @@ LSM303D::init()
 	/* XXX should we enable FIFO? */
 
 	set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */
-	set_antialias_filter_bandwidth(194); /* XXX: choose bandwidth: 50, 194, 362 or 773 Hz */
+	set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
 	set_samplerate(400); /* max sample rate */
 
-	mag_set_range(12); /* XXX: take highest sensor range of 12GA? */
+	mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
 	mag_set_samplerate(100);
 
 	/* XXX test this when another mag is used */
@@ -687,6 +695,17 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* XXX implement */
 		return -EINVAL;
 
+//	case ACCELIOCSLOWPASS:
+	case ACCELIOCGLOWPASS:
+
+		unsigned bandwidth;
+
+		if (OK == get_antialias_filter_bandwidth(bandwidth))
+			return bandwidth;
+		else
+			return -EINVAL;
+
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -942,6 +961,25 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
 	return OK;
 }
 
+int
+LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth)
+{
+	uint8_t readbits = read_reg(ADDR_CTRL_REG2);
+
+	if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A)
+		bandwidth = 50;
+	else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A)
+		bandwidth = 194;
+	else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A)
+		bandwidth = 362;
+	else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A)
+		bandwidth = 773;
+	else
+		return ERROR;
+
+	return OK;
+}
+
 int
 LSM303D::set_samplerate(unsigned frequency)
 {
@@ -1305,6 +1343,7 @@ test()
 	int fd_accel = -1;
 	struct accel_report a_report;
 	ssize_t sz;
+	int filter_bandwidth;
 
 	/* get the driver */
 	fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
@@ -1318,14 +1357,19 @@ test()
 	if (sz != sizeof(a_report))
 		err(1, "immediate read failed");
 
+
 	warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
 	warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
 	warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
 	warnx("accel x: \t%d\traw", (int)a_report.x_raw);
 	warnx("accel y: \t%d\traw", (int)a_report.y_raw);
 	warnx("accel z: \t%d\traw", (int)a_report.z_raw);
-	warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
 
+	warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
+	if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
+		warnx("accel antialias filter bandwidth: fail");
+	else
+		warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth);
 
 	int fd_mag = -1;
 	struct mag_report m_report;
-- 
cgit v1.2.3


From f5f7b3f6dd1c0d48ffb0aa8c78435715a4ce3f5e Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 18 Jul 2013 14:42:08 +0200
Subject: Added scale ioctl to LSM303D driver

---
 src/drivers/lsm303d/lsm303d.cpp | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index ba7316e55..af7746d14 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -705,6 +705,23 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 		else
 			return -EINVAL;
 
+	case ACCELIOCSSCALE:
+		{
+			/* copy scale, but only if off by a few percent */
+			struct accel_scale *s = (struct accel_scale *) arg;
+			float sum = s->x_scale + s->y_scale + s->z_scale;
+			if (sum > 2.0f && sum < 4.0f) {
+				memcpy(&_accel_scale, s, sizeof(_accel_scale));
+				return OK;
+			} else {
+				return -EINVAL;
+			}
+		}
+
+	case ACCELIOCGSCALE:
+		/* copy scale out */
+		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		return OK;
 
 	default:
 		/* give it to the superclass */
-- 
cgit v1.2.3


From eb28f5996f1e102939d9ebb3a562641137ee419b Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 18 Jul 2013 15:21:39 +0200
Subject: Some lost LSM303D fixes for range and scale

---
 src/drivers/lsm303d/lsm303d.cpp | 68 +++++++++++++++++++++++++----------------
 1 file changed, 42 insertions(+), 26 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index af7746d14..14b3d6ab8 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -156,11 +156,11 @@ static const int ERROR = -1;
 #define REG5_RATE_100HZ_M		((1<<4) | (0<<3) | (1<<2))
 #define REG5_RATE_DO_NOT_USE_M	((1<<4) | (1<<3) | (0<<2))
 
-#define REG6_FULL_SCALE_BITS_M	((1<<7) | (1<<6))
-#define REG6_FULL_SCALE_2GA_M	((0<<7) | (0<<6))
-#define REG6_FULL_SCALE_4GA_M	((0<<7) | (1<<6))
-#define REG6_FULL_SCALE_8GA_M	((1<<7) | (0<<6))
-#define REG6_FULL_SCALE_12GA_M	((1<<7) | (1<<6))
+#define REG6_FULL_SCALE_BITS_M	((1<<6) | (1<<5))
+#define REG6_FULL_SCALE_2GA_M	((0<<6) | (0<<5))
+#define REG6_FULL_SCALE_4GA_M	((0<<6) | (1<<5))
+#define REG6_FULL_SCALE_8GA_M	((1<<6) | (0<<5))
+#define REG6_FULL_SCALE_12GA_M	((1<<6) | (1<<5))
 
 #define REG7_CONT_MODE_M		((0<<1) | (0<<0))
 
@@ -218,6 +218,8 @@ private:
 	float				_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
 
+	unsigned			_current_samplerate;
+
 	unsigned			_num_mag_reports;
 	volatile unsigned	_next_mag_report;
 	volatile unsigned	_oldest_mag_report;
@@ -405,8 +407,6 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	// enable debug() calls
 	_debug_enabled = true;
 
-	_mag_range_scale = 12.0f/32767.0f;
-
 	// default scale factors
 	_accel_scale.x_offset = 0;
 	_accel_scale.x_scale  = 1.0f;
@@ -873,37 +873,43 @@ LSM303D::set_range(unsigned max_g)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
-	float new_range = 0.0f;
+	float new_range_g = 0.0f;
+	float new_scale_g_digit = 0.0f;
 
 	if (max_g == 0)
 		max_g = 16;
 
 	if (max_g <= 2) {
-		new_range = 2.0f;
+		new_range_g = 2.0f;
 		setbits |= REG2_FULL_SCALE_2G_A;
+		new_scale_g_digit = 0.061e-3f;
 
 	} else if (max_g <= 4) {
-		new_range = 4.0f;
+		new_range_g = 4.0f;
 		setbits |= REG2_FULL_SCALE_4G_A;
+		new_scale_g_digit = 0.122e-3f;
 
 	} else if (max_g <= 6) {
-		new_range = 6.0f;
+		new_range_g = 6.0f;
 		setbits |= REG2_FULL_SCALE_6G_A;
+		new_scale_g_digit = 0.183e-3f;
 
 	} else if (max_g <= 8) {
-		new_range = 8.0f;
+		new_range_g = 8.0f;
 		setbits |= REG2_FULL_SCALE_8G_A;
+		new_scale_g_digit = 0.244e-3f;
 
 	} else if (max_g <= 16) {
-		new_range = 16.0f;
+		new_range_g = 16.0f;
 		setbits |= REG2_FULL_SCALE_16G_A;
+		new_scale_g_digit = 0.732e-3f;
 
 	} else {
 		return -EINVAL;
 	}
 
-	_accel_range_m_s2 = new_range * 9.80665f;
-	_accel_range_scale = _accel_range_m_s2 / 32768.0f;
+	_accel_range_m_s2 = new_range_g * 9.80665f;
+	_accel_range_scale = new_scale_g_digit * 9.80665f;
 
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
@@ -916,6 +922,7 @@ LSM303D::mag_set_range(unsigned max_ga)
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
 	float new_range = 0.0f;
+	float new_scale_ga_digit = 0.0f;
 
 	if (max_ga == 0)
 		max_ga = 12;
@@ -923,25 +930,29 @@ LSM303D::mag_set_range(unsigned max_ga)
 	if (max_ga <= 2) {
 		new_range = 2.0f;
 		setbits |= REG6_FULL_SCALE_2GA_M;
+		new_scale_ga_digit = 0.080e-3f;
 
 	} else if (max_ga <= 4) {
 		new_range = 4.0f;
 		setbits |= REG6_FULL_SCALE_4GA_M;
+		new_scale_ga_digit = 0.160e-3f;
 
 	} else if (max_ga <= 8) {
 		new_range = 8.0f;
 		setbits |= REG6_FULL_SCALE_8GA_M;
+		new_scale_ga_digit = 0.320e-3f;
 
 	} else if (max_ga <= 12) {
 		new_range = 12.0f;
 		setbits |= REG6_FULL_SCALE_12GA_M;
+		new_scale_ga_digit = 0.479e-3f;
 
 	} else {
 		return -EINVAL;
 	}
 
 	_mag_range_ga = new_range;
-	_mag_range_scale = _mag_range_ga / 32768.0f;
+	_mag_range_scale = new_scale_ga_digit;
 
 	modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
 
@@ -1008,18 +1019,23 @@ LSM303D::set_samplerate(unsigned frequency)
 
 	if (frequency <= 100) {
 		setbits |= REG1_RATE_100HZ_A;
+		_current_samplerate = 100;
 
 	} else if (frequency <= 200) {
 		setbits |= REG1_RATE_200HZ_A;
+		_current_samplerate = 200;
 
 	} else if (frequency <= 400) {
 		setbits |= REG1_RATE_400HZ_A;
+		_current_samplerate = 400;
 
 	} else if (frequency <= 800) {
 		setbits |= REG1_RATE_800HZ_A;
+		_current_samplerate = 800;
 
 	} else if (frequency <= 1600) {
 		setbits |= REG1_RATE_1600HZ_A;
+		_current_samplerate = 1600;
 
 	} else {
 		return -EINVAL;
@@ -1358,7 +1374,7 @@ void
 test()
 {
 	int fd_accel = -1;
-	struct accel_report a_report;
+	struct accel_report accel_report;
 	ssize_t sz;
 	int filter_bandwidth;
 
@@ -1369,20 +1385,20 @@ test()
 		err(1, "%s open failed", ACCEL_DEVICE_PATH);
 
 	/* do a simple demand read */
-	sz = read(fd_accel, &a_report, sizeof(a_report));
+	sz = read(fd_accel, &accel_report, sizeof(accel_report));
 
-	if (sz != sizeof(a_report))
+	if (sz != sizeof(accel_report))
 		err(1, "immediate read failed");
 
 
-	warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
-	warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
-	warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
-	warnx("accel x: \t%d\traw", (int)a_report.x_raw);
-	warnx("accel y: \t%d\traw", (int)a_report.y_raw);
-	warnx("accel z: \t%d\traw", (int)a_report.z_raw);
+	warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x);
+	warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y);
+	warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z);
+	warnx("accel x: \t%d\traw", (int)accel_report.x_raw);
+	warnx("accel y: \t%d\traw", (int)accel_report.y_raw);
+	warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
 
-	warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
+	warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
 	if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
 		warnx("accel antialias filter bandwidth: fail");
 	else
-- 
cgit v1.2.3


From f4df4a4e081d9eaaa5dbeef013fa6320b0cea3f7 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 18 Jul 2013 15:30:39 +0200
Subject: Forgot to add current samplerate to constructor

---
 src/drivers/lsm303d/lsm303d.cpp | 1 +
 1 file changed, 1 insertion(+)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 14b3d6ab8..b157d74c6 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -395,6 +395,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_current_samplerate(0),
 	_num_mag_reports(0),
 	_next_mag_report(0),
 	_oldest_mag_report(0),
-- 
cgit v1.2.3


From da152e148d471ded29857e29040f33c7356c9050 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 18 Jul 2013 16:15:43 +0200
Subject: Added iirFilter to LSM303D

---
 makefiles/config_px4fmu-v1_default.mk |   1 -
 src/drivers/lsm303d/iirFilter.c       | 255 ++++++++++++++++++++++++++++++++++
 src/drivers/lsm303d/iirFilter.h       |  59 ++++++++
 src/drivers/lsm303d/lsm303d.cpp       |  32 ++++-
 src/drivers/lsm303d/module.mk         |   3 +-
 5 files changed, 345 insertions(+), 5 deletions(-)
 create mode 100644 src/drivers/lsm303d/iirFilter.c
 create mode 100644 src/drivers/lsm303d/iirFilter.h

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index d9a699da7..74be1cd23 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -18,7 +18,6 @@ MODULES		+= drivers/led
 MODULES		+= drivers/px4io
 MODULES		+= drivers/px4fmu
 MODULES		+= drivers/boards/px4fmu
-MODULES		+= drivers/lsm303d
 MODULES		+= drivers/ardrone_interface
 MODULES		+= drivers/l3gd20
 MODULES		+= drivers/bma180
diff --git a/src/drivers/lsm303d/iirFilter.c b/src/drivers/lsm303d/iirFilter.c
new file mode 100644
index 000000000..8311f14d6
--- /dev/null
+++ b/src/drivers/lsm303d/iirFilter.c
@@ -0,0 +1,255 @@
+#include "math.h"
+#include "string.h"
+#include "iirFilter.h"
+
+///////////////////////////////////////////////////////////////////////////////
+// Internal function prototypes
+
+int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, double Ts, TF_ZPG_t *pZpgd);
+
+int btDifcToZpgd(const TF_DIF_t *pkDifc, double Ts, TF_ZPG_t *pZpgd);
+
+int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt);
+
+int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly);
+
+///////////////////////////////////////////////////////////////////////////////
+// external functions
+
+int testFunction()
+{
+	printf("TEST\n");
+	return 1;
+}
+
+int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt)
+{
+	TF_POLY_t polyd;
+	TF_ZPG_t zpgd;
+
+	memset(pFilt, 0, sizeof(FIL_T));
+
+	// perform bilinear transform with frequency pre warping
+	btDifcToZpgd(pDifc, Ts, &zpgd);
+
+	// calculate polynom
+	tZpgxToPolyx(&zpgd, &polyd);
+
+	// set the filter parameters
+	tPolydToFil(&polyd, pFilt);
+
+	return 1;
+}
+
+// run filter using inp, return output of the filter
+float updateFilter(FIL_T *pFilt, float inp)
+{
+	float outp = 0;
+	int idx; // index used for different things
+	int i; // loop variable
+
+	// Store the input to the input array
+	idx = pFilt->inpCnt % MAX_LENGTH;
+	pFilt->inpData[idx] = inp;
+
+	// calculate numData * inpData
+	for (i = 0; i < pFilt->numLength; i++)
+	{
+		// index of inp array
+		idx = (pFilt->inpCnt + i - pFilt->numLength + 1) % MAX_LENGTH;
+		outp += pFilt->numData[i] * pFilt->inpData[idx];
+	}
+
+	// calculate denData * outData
+	for (i = 0; i < pFilt->denLength; i++)
+	{
+		// index of inp array
+		idx = (pFilt->inpCnt + i - pFilt->denLength) % MAX_LENGTH;
+		outp -= pFilt->denData[i] * pFilt->outData[idx];
+	}
+
+	// store the ouput data to the output array
+	idx = pFilt->inpCnt % MAX_LENGTH;
+	pFilt->outData[idx] = outp;
+
+	pFilt->inpCnt += 1;
+
+	return outp;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Internal functions
+
+int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt)
+{
+	double gain;
+	int i;
+
+	if (pkPolyd->Ts < 0)
+	{
+		return 0;
+	}
+
+	// initialize filter to 0
+	memset(pFilt, 0, sizeof(FIL_T));
+
+	gain = pkPolyd->denData[pkPolyd->denLength - 1];
+	pFilt->Ts = pkPolyd->Ts;
+
+	pFilt->denLength = pkPolyd->denLength - 1;
+	pFilt->numLength = pkPolyd->numLength;
+
+	for (i = 0; i < pkPolyd->numLength; i++)
+	{
+		pFilt->numData[i] = pkPolyd->numData[i] / gain;
+	}
+
+	for (i = 0; i < (pkPolyd->denLength - 1); i++)
+	{
+		pFilt->denData[i] = pkPolyd->denData[i] / gain;
+	}
+}
+
+// bilinear transformation of poles and zeros
+int btDifcToZpgd(const TF_DIF_t *pkDifc,
+				 double Ts,
+				 TF_ZPG_t *pZpgd)
+{
+	TF_ZPG_t zpgc;
+	int totDiff;
+	int i;
+
+	zpgc.zerosLength = 0;
+	zpgc.polesLength = 0;
+	zpgc.gain = pkDifc->gain;
+	zpgc.Ts = pkDifc->Ts;
+
+	// Total number of differentiators / integerators
+	// positive diff, negative integ, 0 for no int/diff
+	totDiff = pkDifc->numDiff - pkDifc->numInt + pkDifc->highpassLength;
+
+	while (zpgc.zerosLength < totDiff)
+	{
+		zpgc.zerosData[zpgc.zerosLength] = 0;
+		zpgc.zerosLength++;
+	}
+	while (zpgc.polesLength < -totDiff)
+	{
+		zpgc.polesData[zpgc.polesLength] = 0;
+		zpgc.polesLength++;
+	}
+
+	// The next step is to calculate the poles
+	// This has to be done for the highpass and lowpass filters
+	// As we are using bilinear transformation there will be frequency
+	// warping which has to be accounted for
+	for (i = 0; i < pkDifc->lowpassLength; i++)
+	{
+		// pre-warping:
+		double freq = 2.0 / Ts * tan(pkDifc->lowpassData[i] * 2.0 * M_PI * Ts / 2.0);
+		// calculate pole:
+		zpgc.polesData[zpgc.polesLength] = -freq;
+		// adjust gain such that lp has gain = 1 for low frequencies
+		zpgc.gain *= freq;
+		zpgc.polesLength++;
+	}
+	for (i = 0; i < pkDifc->highpassLength; i++)
+	{
+		// pre-warping
+		double freq = 2.0 / Ts * tan(pkDifc->highpassData[i] * 2.0 * M_PI * Ts / 2.0);
+		// calculate pole:
+		zpgc.polesData[zpgc.polesLength] = -freq;
+		// gain does not have to be adjusted
+		zpgc.polesLength++;
+	}
+
+	return btZpgcToZpgd(&zpgc, Ts, pZpgd);
+}
+
+// bilinear transformation of poles and zeros
+int btZpgcToZpgd(const TF_ZPG_t *pkZpgc,
+				 double Ts,
+				 TF_ZPG_t *pZpgd)
+{
+	int i;
+
+	// init digital gain
+	pZpgd->gain = pkZpgc->gain;
+
+	// transform the poles
+	pZpgd->polesLength = pkZpgc->polesLength;
+	for (i = 0; i < pkZpgc->polesLength; i++)
+	{
+		pZpgd->polesData[i] = (2.0 / Ts + pkZpgc->polesData[i]) / (2.0 / Ts - pkZpgc->polesData[i]);
+		pZpgd->gain /= (2.0 / Ts - pkZpgc->polesData[i]);
+	}
+
+	// transform the zeros
+	pZpgd->zerosLength = pkZpgc->zerosLength;
+	for (i = 0; i < pkZpgc->zerosLength; i++)
+	{
+		pZpgd->zerosData[i] = (2.0 / Ts + pkZpgc->zerosData[i]) / (2.0 / Ts + pkZpgc->zerosData[i]);
+		pZpgd->gain *= (2.0 / Ts - pkZpgc->zerosData[i]);
+	}
+
+	// if we don't have the same number of poles as zeros we have to add
+	// poles or zeros due to the bilinear transformation
+	while (pZpgd->zerosLength < pZpgd->polesLength)
+	{
+		pZpgd->zerosData[pZpgd->zerosLength] = -1.0;
+		pZpgd->zerosLength++;
+	}
+	while (pZpgd->zerosLength > pZpgd->polesLength)
+	{
+		pZpgd->polesData[pZpgd->polesLength] = -1.0;
+		pZpgd->polesLength++;
+	}
+
+	pZpgd->Ts = Ts;
+
+	return 1;
+}
+
+// calculate polynom from zero, pole, gain form
+int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly)
+{
+	int i, j; // counter variable
+	double tmp0, tmp1, gain;
+
+	// copy Ts
+	pPoly->Ts = pkZpg->Ts;
+
+	// calculate denominator polynom
+	pPoly->denLength = 1;
+	pPoly->denData[0] = 1.0;
+	for (i = 0; i < pkZpg->polesLength; i++)
+	{
+		// init temporary variable
+		tmp0 = 0.0;
+		// increase the polynom by 1
+		pPoly->denData[pPoly->denLength] = 0;
+		pPoly->denLength++;
+		for (j = 0; j < pPoly->denLength; j++)
+		{
+			tmp1 = pPoly->denData[j];
+			pPoly->denData[j] = tmp1 * -pkZpg->polesData[i] + tmp0;
+			tmp0 = tmp1;
+		}
+	}
+
+	// calculate numerator polynom
+	pPoly->numLength = 1;
+	pPoly->numData[0] = pkZpg->gain;
+	for (i = 0; i < pkZpg->zerosLength; i++)
+	{
+		tmp0 = 0.0;
+		pPoly->numData[pPoly->numLength] = 0;
+		pPoly->numLength++;
+		for (j = 0; j < pPoly->numLength; j++)
+		{
+			tmp1 = pPoly->numData[j];
+			pPoly->numData[j] = tmp1 * -pkZpg->zerosData[i] + tmp0;
+			tmp0 = tmp1;
+		}
+	}
+}
diff --git a/src/drivers/lsm303d/iirFilter.h b/src/drivers/lsm303d/iirFilter.h
new file mode 100644
index 000000000..ab4223a8e
--- /dev/null
+++ b/src/drivers/lsm303d/iirFilter.h
@@ -0,0 +1,59 @@
+#ifndef IIRFILTER__H__
+#define IIRFILTER__H__
+
+__BEGIN_DECLS
+
+#define MAX_LENGTH 4
+
+typedef struct FILTER_s
+{
+	float denData[MAX_LENGTH];
+	float numData[MAX_LENGTH];
+	int denLength;
+	int numLength;
+	float Ts;
+	float inpData[MAX_LENGTH];
+	float outData[MAX_LENGTH];
+	unsigned int inpCnt;
+} FIL_T;
+
+typedef struct TF_ZPG_s
+{
+	int    zerosLength;
+	double zerosData[MAX_LENGTH + 1];
+	int    polesLength;
+	double polesData[MAX_LENGTH + 1];
+	double gain;
+	double Ts;
+} TF_ZPG_t;
+
+typedef struct TF_POLY_s
+{
+	int    numLength;
+	double numData[MAX_LENGTH];
+	int    denLength;
+	double denData[MAX_LENGTH];
+	double Ts;
+} TF_POLY_t;
+
+typedef struct TF_DIF_s
+{
+	int    numInt;
+	int    numDiff;
+	int    lowpassLength;
+	int    highpassLength;
+	double lowpassData[MAX_LENGTH];
+	int    highpassData[MAX_LENGTH];
+	double gain;
+	double Ts;
+} TF_DIF_t;
+
+__EXPORT int testFunction(void);
+
+__EXPORT float updateFilter(FIL_T *pFilt, float inp);
+
+__EXPORT int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt);
+
+__END_DECLS
+
+#endif
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index b157d74c6..80d777826 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -65,6 +65,7 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 
+#include "iirFilter.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -220,6 +221,10 @@ private:
 
 	unsigned			_current_samplerate;
 
+	FIL_T				_filter_x;
+	FIL_T				_filter_y;
+	FIL_T				_filter_z;
+
 	unsigned			_num_mag_reports;
 	volatile unsigned	_next_mag_report;
 	volatile unsigned	_oldest_mag_report;
@@ -489,6 +494,22 @@ LSM303D::init()
 	set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
 	set_samplerate(400); /* max sample rate */
 
+	/* initiate filter */
+	TF_DIF_t tf_filter;
+	tf_filter.numInt = 0;
+	tf_filter.numDiff = 0;
+	tf_filter.lowpassLength = 2;
+	tf_filter.highpassLength = 0;
+	tf_filter.lowpassData[0] = 10;
+	tf_filter.lowpassData[1] = 10;
+	//tf_filter.highpassData[0] = ;
+	tf_filter.gain = 1;
+	tf_filter.Ts = 1/_current_samplerate;
+
+	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x);
+	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y);
+	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z);
+
 	mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
 	mag_set_samplerate(100);
 
@@ -1161,9 +1182,14 @@ LSM303D::measure()
 	accel_report->y_raw = raw_accel_report.y;
 	accel_report->z_raw = raw_accel_report.z;
 
-	accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	
+	accel_report->x = updateFilter(&_filter_x, x_in_new);
+	accel_report->y = updateFilter(&_filter_y, y_in_new);
+	accel_report->z = updateFilter(&_filter_z, z_in_new);
+
 	accel_report->scaling = _accel_range_scale;
 	accel_report->range_m_s2 = _accel_range_m_s2;
 
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
index e93b1e331..8fd5679c9 100644
--- a/src/drivers/lsm303d/module.mk
+++ b/src/drivers/lsm303d/module.mk
@@ -3,4 +3,5 @@
 #
 
 MODULE_COMMAND	 = lsm303d
-SRCS		 = lsm303d.cpp
+SRCS		 = lsm303d.cpp \
+		iirFilter.c
-- 
cgit v1.2.3


From e309b9ab4a633065f06a0f0c66542df84e6147d5 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 18 Jul 2013 19:45:44 +0200
Subject: Disable IIR filter for now

---
 src/drivers/lsm303d/lsm303d.cpp | 52 ++++++++++++++++++++++-------------------
 1 file changed, 28 insertions(+), 24 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 80d777826..a9a9bb69f 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -221,9 +221,9 @@ private:
 
 	unsigned			_current_samplerate;
 
-	FIL_T				_filter_x;
-	FIL_T				_filter_y;
-	FIL_T				_filter_z;
+//	FIL_T				_filter_x;
+//	FIL_T				_filter_y;
+//	FIL_T				_filter_z;
 
 	unsigned			_num_mag_reports;
 	volatile unsigned	_next_mag_report;
@@ -495,20 +495,20 @@ LSM303D::init()
 	set_samplerate(400); /* max sample rate */
 
 	/* initiate filter */
-	TF_DIF_t tf_filter;
-	tf_filter.numInt = 0;
-	tf_filter.numDiff = 0;
-	tf_filter.lowpassLength = 2;
-	tf_filter.highpassLength = 0;
-	tf_filter.lowpassData[0] = 10;
-	tf_filter.lowpassData[1] = 10;
-	//tf_filter.highpassData[0] = ;
-	tf_filter.gain = 1;
-	tf_filter.Ts = 1/_current_samplerate;
-
-	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x);
-	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y);
-	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z);
+//	TF_DIF_t tf_filter;
+//	tf_filter.numInt = 0;
+//	tf_filter.numDiff = 0;
+//	tf_filter.lowpassLength = 2;
+//	tf_filter.highpassLength = 0;
+//	tf_filter.lowpassData[0] = 10;
+//	tf_filter.lowpassData[1] = 10;
+//	//tf_filter.highpassData[0] = ;
+//	tf_filter.gain = 1;
+//	tf_filter.Ts = 1/_current_samplerate;
+//
+//	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x);
+//	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y);
+//	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z);
 
 	mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
 	mag_set_samplerate(100);
@@ -1182,13 +1182,17 @@ LSM303D::measure()
 	accel_report->y_raw = raw_accel_report.y;
 	accel_report->z_raw = raw_accel_report.z;
 
-	float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	
-	accel_report->x = updateFilter(&_filter_x, x_in_new);
-	accel_report->y = updateFilter(&_filter_y, y_in_new);
-	accel_report->z = updateFilter(&_filter_z, z_in_new);
+//	float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+//	float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+//	float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+//
+//	accel_report->x = updateFilter(&_filter_x, x_in_new);
+//	accel_report->y = updateFilter(&_filter_y, y_in_new);
+//	accel_report->z = updateFilter(&_filter_z, z_in_new);
+
+	accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
 
 	accel_report->scaling = _accel_range_scale;
 	accel_report->range_m_s2 = _accel_range_m_s2;
-- 
cgit v1.2.3


From 374d204b9223b5b3c33ec1fd48a120a9984160c2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 18 Jul 2013 19:48:15 +0200
Subject: Enable BDU instead of CONT mode

---
 src/drivers/lsm303d/lsm303d.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index a9a9bb69f..844cc3884 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -125,7 +125,7 @@ static const int ERROR = -1;
 #define REG1_RATE_800HZ_A		((1<<7) | (0<<6) | (0<<5) | (1<<4))
 #define REG1_RATE_1600HZ_A		((1<<7) | (0<<6) | (1<<5) | (0<<4))
 
-#define REG1_CONT_UPDATE_A		(0<<3)
+#define REG1_BDU_UPDATE			(1<<3)
 #define REG1_Z_ENABLE_A			(1<<2)
 #define REG1_Y_ENABLE_A			(1<<1)
 #define REG1_X_ENABLE_A			(1<<0)
@@ -482,7 +482,7 @@ LSM303D::init()
 	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
 
 	/* enable accel, XXX do this with an ioctl? */
-	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
+	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
 
 	/* enable mag, XXX do this with an ioctl? */
 	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
-- 
cgit v1.2.3


From 9d6ec6b3655fcd902be7a7fe4f2a24033c714afb Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Fri, 2 Aug 2013 22:34:55 -0700
Subject: Restructure things so that the PX4 configs move out of the NuttX
 tree, and most of the PX4-specific board configuration data moves out of the
 config and into the board driver.

Rename some directories that got left behind in the great board renaming.
---
 Makefile                                        |   2 +-
 makefiles/config_px4fmu-v1_default.mk           |   2 +-
 makefiles/config_px4fmu-v2_default.mk           |   7 +-
 makefiles/config_px4fmu-v2_test.mk              |   2 +-
 makefiles/config_px4io-v1_default.mk            |   2 +-
 makefiles/config_px4io-v2_default.mk            |   2 +-
 makefiles/firmware.mk                           |   5 +
 makefiles/setup.mk                              |   1 +
 nuttx-configs/px4fmu-v1/Kconfig                 |  21 +
 nuttx-configs/px4fmu-v1/common/Make.defs        | 184 +++++
 nuttx-configs/px4fmu-v1/common/ld.script        | 149 ++++
 nuttx-configs/px4fmu-v1/include/board.h         | 319 ++++++++
 nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h  |  42 ++
 nuttx-configs/px4fmu-v1/nsh/Make.defs           |   3 +
 nuttx-configs/px4fmu-v1/nsh/defconfig           | 957 ++++++++++++++++++++++++
 nuttx-configs/px4fmu-v1/nsh/setenv.sh           |  75 ++
 nuttx-configs/px4fmu-v1/ostest/Make.defs        | 122 +++
 nuttx-configs/px4fmu-v1/ostest/defconfig        | 583 +++++++++++++++
 nuttx-configs/px4fmu-v1/ostest/setenv.sh        |  75 ++
 nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld      | 129 ++++
 nuttx-configs/px4fmu-v1/scripts/ld.script       | 123 +++
 nuttx-configs/px4fmu-v1/src/Makefile            |  84 +++
 nuttx-configs/px4fmu-v1/src/empty.c             |   4 +
 nuttx-configs/px4fmu-v1/tools/px_mkfw.py        | 110 +++
 nuttx-configs/px4fmu-v1/tools/px_uploader.py    | 416 ++++++++++
 nuttx-configs/px4fmu-v1/tools/upload.sh         |  27 +
 nuttx-configs/px4fmu-v2/include/board.h         |  58 --
 nuttx-configs/px4io-v1/Kconfig                  |  21 +
 nuttx-configs/px4io-v1/README.txt               | 806 ++++++++++++++++++++
 nuttx-configs/px4io-v1/common/Make.defs         | 171 +++++
 nuttx-configs/px4io-v1/common/ld.script         | 129 ++++
 nuttx-configs/px4io-v1/common/setenv.sh         |  47 ++
 nuttx-configs/px4io-v1/include/README.txt       |   1 +
 nuttx-configs/px4io-v1/include/board.h          | 152 ++++
 nuttx-configs/px4io-v1/include/drv_i2c_device.h |  42 ++
 nuttx-configs/px4io-v1/nsh/Make.defs            |   3 +
 nuttx-configs/px4io-v1/nsh/appconfig            |  32 +
 nuttx-configs/px4io-v1/nsh/defconfig            | 559 ++++++++++++++
 nuttx-configs/px4io-v1/nsh/setenv.sh            |  47 ++
 nuttx-configs/px4io-v1/src/Makefile             |  84 +++
 nuttx-configs/px4io-v1/src/README.txt           |   1 +
 nuttx-configs/px4io-v1/src/drv_i2c_device.c     | 618 +++++++++++++++
 nuttx-configs/px4io-v1/src/empty.c              |   4 +
 nuttx-configs/px4io-v2/include/board.h          |  28 -
 src/drivers/blinkm/blinkm.cpp                   |  16 +-
 src/drivers/bma180/bma180.cpp                   |   2 +-
 src/drivers/boards/px4fmu-v1/board_config.h     | 209 ++++++
 src/drivers/boards/px4fmu-v1/module.mk          |  10 +
 src/drivers/boards/px4fmu-v1/px4fmu_can.c       | 144 ++++
 src/drivers/boards/px4fmu-v1/px4fmu_init.c      | 268 +++++++
 src/drivers/boards/px4fmu-v1/px4fmu_led.c       |  96 +++
 src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c |  87 +++
 src/drivers/boards/px4fmu-v1/px4fmu_spi.c       | 154 ++++
 src/drivers/boards/px4fmu-v1/px4fmu_usb.c       | 108 +++
 src/drivers/boards/px4fmu-v2/board_config.h     | 192 +++++
 src/drivers/boards/px4fmu-v2/module.mk          |  10 +
 src/drivers/boards/px4fmu-v2/px4fmu2_init.c     | 262 +++++++
 src/drivers/boards/px4fmu-v2/px4fmu2_led.c      |  85 +++
 src/drivers/boards/px4fmu-v2/px4fmu_can.c       | 144 ++++
 src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c | 105 +++
 src/drivers/boards/px4fmu-v2/px4fmu_spi.c       | 141 ++++
 src/drivers/boards/px4fmu-v2/px4fmu_usb.c       | 108 +++
 src/drivers/boards/px4fmu/module.mk             |  10 -
 src/drivers/boards/px4fmu/px4fmu_can.c          | 144 ----
 src/drivers/boards/px4fmu/px4fmu_init.c         | 268 -------
 src/drivers/boards/px4fmu/px4fmu_internal.h     | 165 ----
 src/drivers/boards/px4fmu/px4fmu_led.c          |  96 ---
 src/drivers/boards/px4fmu/px4fmu_pwm_servo.c    |  87 ---
 src/drivers/boards/px4fmu/px4fmu_spi.c          | 154 ----
 src/drivers/boards/px4fmu/px4fmu_usb.c          | 108 ---
 src/drivers/boards/px4fmuv2/module.mk           |  10 -
 src/drivers/boards/px4fmuv2/px4fmu2_init.c      | 262 -------
 src/drivers/boards/px4fmuv2/px4fmu2_led.c       |  85 ---
 src/drivers/boards/px4fmuv2/px4fmu_can.c        | 144 ----
 src/drivers/boards/px4fmuv2/px4fmu_internal.h   | 143 ----
 src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c  | 105 ---
 src/drivers/boards/px4fmuv2/px4fmu_spi.c        | 141 ----
 src/drivers/boards/px4fmuv2/px4fmu_usb.c        | 108 ---
 src/drivers/boards/px4io-v1/board_config.h      |  95 +++
 src/drivers/boards/px4io-v1/module.mk           |   6 +
 src/drivers/boards/px4io-v1/px4io_init.c        | 106 +++
 src/drivers/boards/px4io-v1/px4io_pwm_servo.c   | 123 +++
 src/drivers/boards/px4io-v2/board_config.h      | 138 ++++
 src/drivers/boards/px4io-v2/module.mk           |   6 +
 src/drivers/boards/px4io-v2/px4iov2_init.c      | 162 ++++
 src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c | 123 +++
 src/drivers/boards/px4io/module.mk              |   6 -
 src/drivers/boards/px4io/px4io_init.c           | 106 ---
 src/drivers/boards/px4io/px4io_internal.h       |  85 ---
 src/drivers/boards/px4io/px4io_pwm_servo.c      | 123 ---
 src/drivers/boards/px4iov2/module.mk            |   6 -
 src/drivers/boards/px4iov2/px4iov2_init.c       | 162 ----
 src/drivers/boards/px4iov2/px4iov2_internal.h   | 118 ---
 src/drivers/boards/px4iov2/px4iov2_pwm_servo.c  | 123 ---
 src/drivers/drv_gpio.h                          |   8 +-
 src/drivers/ets_airspeed/ets_airspeed.cpp       |   2 +-
 src/drivers/hmc5883/hmc5883.cpp                 |   2 +-
 src/drivers/l3gd20/l3gd20.cpp                   |   3 +-
 src/drivers/lsm303d/lsm303d.cpp                 |   4 +-
 src/drivers/mb12xx/mb12xx.cpp                   |   4 +-
 src/drivers/meas_airspeed/meas_airspeed.cpp     |   2 +-
 src/drivers/mkblctrl/mkblctrl.cpp               |   3 +-
 src/drivers/mpu6000/mpu6000.cpp                 |   2 +-
 src/drivers/px4fmu/fmu.cpp                      |  16 +-
 src/drivers/px4io/px4io_serial.cpp              |   2 +-
 src/drivers/px4io/px4io_uploader.cpp            |   7 +-
 src/drivers/rgbled/rgbled.cpp                   |   3 +-
 src/drivers/stm32/drv_hrt.c                     |   7 +-
 src/drivers/stm32/tone_alarm/tone_alarm.cpp     |   2 +-
 src/modules/px4iofirmware/px4io.h               |   7 +-
 src/modules/systemlib/systemlib.c               |   4 +-
 src/systemcmds/eeprom/eeprom.c                  |   2 +-
 src/systemcmds/i2c/i2c.c                        |   2 +-
 113 files changed, 9075 insertions(+), 2915 deletions(-)
 create mode 100644 nuttx-configs/px4fmu-v1/Kconfig
 create mode 100644 nuttx-configs/px4fmu-v1/common/Make.defs
 create mode 100644 nuttx-configs/px4fmu-v1/common/ld.script
 create mode 100644 nuttx-configs/px4fmu-v1/include/board.h
 create mode 100644 nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h
 create mode 100644 nuttx-configs/px4fmu-v1/nsh/Make.defs
 create mode 100644 nuttx-configs/px4fmu-v1/nsh/defconfig
 create mode 100755 nuttx-configs/px4fmu-v1/nsh/setenv.sh
 create mode 100644 nuttx-configs/px4fmu-v1/ostest/Make.defs
 create mode 100644 nuttx-configs/px4fmu-v1/ostest/defconfig
 create mode 100755 nuttx-configs/px4fmu-v1/ostest/setenv.sh
 create mode 100644 nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld
 create mode 100644 nuttx-configs/px4fmu-v1/scripts/ld.script
 create mode 100644 nuttx-configs/px4fmu-v1/src/Makefile
 create mode 100644 nuttx-configs/px4fmu-v1/src/empty.c
 create mode 100755 nuttx-configs/px4fmu-v1/tools/px_mkfw.py
 create mode 100755 nuttx-configs/px4fmu-v1/tools/px_uploader.py
 create mode 100755 nuttx-configs/px4fmu-v1/tools/upload.sh
 create mode 100644 nuttx-configs/px4io-v1/Kconfig
 create mode 100755 nuttx-configs/px4io-v1/README.txt
 create mode 100644 nuttx-configs/px4io-v1/common/Make.defs
 create mode 100755 nuttx-configs/px4io-v1/common/ld.script
 create mode 100755 nuttx-configs/px4io-v1/common/setenv.sh
 create mode 100755 nuttx-configs/px4io-v1/include/README.txt
 create mode 100755 nuttx-configs/px4io-v1/include/board.h
 create mode 100644 nuttx-configs/px4io-v1/include/drv_i2c_device.h
 create mode 100644 nuttx-configs/px4io-v1/nsh/Make.defs
 create mode 100644 nuttx-configs/px4io-v1/nsh/appconfig
 create mode 100755 nuttx-configs/px4io-v1/nsh/defconfig
 create mode 100755 nuttx-configs/px4io-v1/nsh/setenv.sh
 create mode 100644 nuttx-configs/px4io-v1/src/Makefile
 create mode 100644 nuttx-configs/px4io-v1/src/README.txt
 create mode 100644 nuttx-configs/px4io-v1/src/drv_i2c_device.c
 create mode 100644 nuttx-configs/px4io-v1/src/empty.c
 create mode 100644 src/drivers/boards/px4fmu-v1/board_config.h
 create mode 100644 src/drivers/boards/px4fmu-v1/module.mk
 create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_can.c
 create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_init.c
 create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_led.c
 create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
 create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_spi.c
 create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_usb.c
 create mode 100644 src/drivers/boards/px4fmu-v2/board_config.h
 create mode 100644 src/drivers/boards/px4fmu-v2/module.mk
 create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu2_init.c
 create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu2_led.c
 create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu_can.c
 create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
 create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu_spi.c
 create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu_usb.c
 delete mode 100644 src/drivers/boards/px4fmu/module.mk
 delete mode 100644 src/drivers/boards/px4fmu/px4fmu_can.c
 delete mode 100644 src/drivers/boards/px4fmu/px4fmu_init.c
 delete mode 100644 src/drivers/boards/px4fmu/px4fmu_internal.h
 delete mode 100644 src/drivers/boards/px4fmu/px4fmu_led.c
 delete mode 100644 src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
 delete mode 100644 src/drivers/boards/px4fmu/px4fmu_spi.c
 delete mode 100644 src/drivers/boards/px4fmu/px4fmu_usb.c
 delete mode 100644 src/drivers/boards/px4fmuv2/module.mk
 delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu2_init.c
 delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu2_led.c
 delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_can.c
 delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_internal.h
 delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c
 delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_spi.c
 delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_usb.c
 create mode 100644 src/drivers/boards/px4io-v1/board_config.h
 create mode 100644 src/drivers/boards/px4io-v1/module.mk
 create mode 100644 src/drivers/boards/px4io-v1/px4io_init.c
 create mode 100644 src/drivers/boards/px4io-v1/px4io_pwm_servo.c
 create mode 100644 src/drivers/boards/px4io-v2/board_config.h
 create mode 100644 src/drivers/boards/px4io-v2/module.mk
 create mode 100644 src/drivers/boards/px4io-v2/px4iov2_init.c
 create mode 100644 src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
 delete mode 100644 src/drivers/boards/px4io/module.mk
 delete mode 100644 src/drivers/boards/px4io/px4io_init.c
 delete mode 100644 src/drivers/boards/px4io/px4io_internal.h
 delete mode 100644 src/drivers/boards/px4io/px4io_pwm_servo.c
 delete mode 100644 src/drivers/boards/px4iov2/module.mk
 delete mode 100644 src/drivers/boards/px4iov2/px4iov2_init.c
 delete mode 100755 src/drivers/boards/px4iov2/px4iov2_internal.h
 delete mode 100644 src/drivers/boards/px4iov2/px4iov2_pwm_servo.c

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/Makefile b/Makefile
index a703bef4c..6f58858f0 100644
--- a/Makefile
+++ b/Makefile
@@ -148,7 +148,7 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC)
 	@$(ECHO) %% Configuring NuttX for $(board)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
 	$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
-	$(Q) (cd $(NUTTX_SRC)/configs && ln -sf $(PX4_BASE)/nuttx-configs/* .)
+	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)/nuttx-configs/* .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration))
 	@$(ECHO) %% Exporting NuttX for $(board)
 	$(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index e6ec840f9..255093e67 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -17,7 +17,7 @@ MODULES		+= drivers/stm32/tone_alarm
 MODULES		+= drivers/led
 MODULES		+= drivers/px4io
 MODULES		+= drivers/px4fmu
-MODULES		+= drivers/boards/px4fmu
+MODULES		+= drivers/boards/px4fmu-v1
 MODULES		+= drivers/ardrone_interface
 MODULES		+= drivers/l3gd20
 MODULES		+= drivers/bma180
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 4add744d0..75573e2c2 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -17,7 +17,7 @@ MODULES		+= drivers/stm32/tone_alarm
 MODULES		+= drivers/led
 MODULES		+= drivers/px4fmu
 MODULES		+= drivers/px4io
-MODULES		+= drivers/boards/px4fmuv2
+MODULES		+= drivers/boards/px4fmu-v2
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/lsm303d
 MODULES		+= drivers/l3gd20
@@ -29,12 +29,15 @@ MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
-MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
 MODULES		+= modules/sensors
 
+# Needs to be burned to the ground and re-written; for now,
+# just don't build it.
+#MODULES		+= drivers/mkblctrl
+
 #
 # System commands
 #
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 98fd6feda..0f60e88b5 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -13,7 +13,7 @@ ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_test
 MODULES		+= drivers/device
 MODULES		+= drivers/stm32
 MODULES		+= drivers/led
-MODULES		+= drivers/boards/px4fmuv2
+MODULES		+= drivers/boards/px4fmu-v2
 MODULES		+= systemcmds/perf
 MODULES		+= systemcmds/reboot
 
diff --git a/makefiles/config_px4io-v1_default.mk b/makefiles/config_px4io-v1_default.mk
index cf70391bc..73f8adf20 100644
--- a/makefiles/config_px4io-v1_default.mk
+++ b/makefiles/config_px4io-v1_default.mk
@@ -6,5 +6,5 @@
 # Board support modules
 #
 MODULES		+= drivers/stm32
-MODULES		+= drivers/boards/px4io
+MODULES		+= drivers/boards/px4io-v1
 MODULES		+= modules/px4iofirmware
\ No newline at end of file
diff --git a/makefiles/config_px4io-v2_default.mk b/makefiles/config_px4io-v2_default.mk
index f9f35d629..dbeaba3d3 100644
--- a/makefiles/config_px4io-v2_default.mk
+++ b/makefiles/config_px4io-v2_default.mk
@@ -6,5 +6,5 @@
 # Board support modules
 #
 MODULES		+= drivers/stm32
-MODULES		+= drivers/boards/px4iov2
+MODULES		+= drivers/boards/px4io-v2
 MODULES		+= modules/px4iofirmware
\ No newline at end of file
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index 3ad13088b..2085d45dd 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -176,6 +176,11 @@ GLOBAL_DEPS		+= $(MAKEFILE_LIST)
 #
 EXTRA_CLEANS		 = 
 
+#
+# Append the per-board driver directory to the header search path.
+#
+INCLUDE_DIRS		+= $(PX4_MODULE_SRC)drivers/boards/$(BOARD)
+
 ################################################################################
 # NuttX libraries and paths
 ################################################################################
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index a0e880a0d..fdb161201 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -64,6 +64,7 @@ export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 export MKFW		 = $(PX4_BASE)/Tools/px_mkfw.py
 export UPLOADER		 = $(PX4_BASE)/Tools/px_uploader.py
 export COPY		 = cp
+export COPYDIR		 = cp -Rf
 export REMOVE		 = rm -f
 export RMDIR		 = rm -rf
 export GENROMFS		 = genromfs
diff --git a/nuttx-configs/px4fmu-v1/Kconfig b/nuttx-configs/px4fmu-v1/Kconfig
new file mode 100644
index 000000000..edbafa06f
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/Kconfig
@@ -0,0 +1,21 @@
+#
+# For a description of the syntax of this configuration file,
+# see misc/tools/kconfig-language.txt.
+#
+
+if ARCH_BOARD_PX4FMU_V1
+
+config HRT_TIMER
+	bool "High resolution timer support"
+	default y
+	---help---
+		Enable high resolution timer for PPM capture and system clocks.
+
+config HRT_PPM
+	bool "PPM input capture"
+	default y
+	depends on HRT_TIMER
+	---help---
+		Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs)
+
+endif
diff --git a/nuttx-configs/px4fmu-v1/common/Make.defs b/nuttx-configs/px4fmu-v1/common/Make.defs
new file mode 100644
index 000000000..756286ccb
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/common/Make.defs
@@ -0,0 +1,184 @@
+############################################################################
+# configs/px4fmu/common/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+#
+# Generic Make.defs for the PX4FMU
+# Do not specify/use this file directly - it is included by config-specific
+# Make.defs in the per-config directories.
+#
+
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -O3
+ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
+			   -mthumb \
+			   -march=armv7e-m \
+			   -mfpu=fpv4-sp-d16 \
+			   -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+INSTRUMENTATIONDEFINES	 = -finstrument-functions \
+			   -ffixed-r10
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS		+= $(LIBM)
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain 
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+   			   -funsafe-math-optimizations \
+   			   -fno-builtin-printf \
+   			   -ffunction-sections \
+   			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = -Wall \
+			   -Wextra \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wfloat-equal \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wmissing-declarations \
+			   -Wpacked \
+			   -Wno-unused-parameter
+#   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
+#   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+#   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS		 = $(ARCHWARNINGS) \
+			   -Wbad-function-cast \
+			   -Wstrict-prototypes \
+			   -Wold-style-declaration \
+			   -Wmissing-parameter-type \
+			   -Wmissing-prototypes \
+			   -Wnested-externs \
+			   -Wunsuffixed-float-constants
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
+			   -Wno-psabi
+ARCHDEFINES		 =
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+
diff --git a/nuttx-configs/px4fmu-v1/common/ld.script b/nuttx-configs/px4fmu-v1/common/ld.script
new file mode 100644
index 000000000..de8179e8d
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/common/ld.script
@@ -0,0 +1,149 @@
+/****************************************************************************
+ * configs/px4fmu/common/ld.script
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F405 has 1024Kb of FLASH beginning at address 0x0800:0000 and
+ * 192Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 1008K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 128K
+    ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/* 
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)        
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)        
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+		/* 
+		 * This is a hack to make the newlib libm __errno() call
+		 * use the NuttX get_errno_ptr() function.
+		 */
+		__errno = get_errno_ptr;
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h
new file mode 100644
index 000000000..1921f7715
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/include/board.h
@@ -0,0 +1,319 @@
+/************************************************************************************
+ * configs/stm32f4discovery/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
+#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include "stm32_rcc.h"
+#include "stm32_sdio.h"
+#include "stm32.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The PX4FMU uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the canonical configuration:
+ *   System Clock source           : PLL (HSE)
+ *   SYSCLK(Hz)                    : 168000000    Determined by PLL configuration
+ *   HCLK(Hz)                      : 168000000    (STM32_RCC_CFGR_HPRE)
+ *   AHB Prescaler                 : 1            (STM32_RCC_CFGR_HPRE)
+ *   APB1 Prescaler                : 4            (STM32_RCC_CFGR_PPRE1)
+ *   APB2 Prescaler                : 2            (STM32_RCC_CFGR_PPRE2)
+ *   HSE Frequency(Hz)             : 24000000     (STM32_BOARD_XTAL)
+ *   PLLM                          : 24           (STM32_PLLCFG_PLLM)
+ *   PLLN                          : 336          (STM32_PLLCFG_PLLN)
+ *   PLLP                          : 2            (STM32_PLLCFG_PLLP)
+ *   PLLQ                          : 7            (STM32_PLLCFG_PLLQ)
+ *   Main regulator output voltage : Scale1 mode  Needed for high speed SYSCLK
+ *   Flash Latency(WS)             : 5
+ *   Prefetch Buffer               : OFF
+ *   Instruction cache             : ON
+ *   Data cache                    : ON
+ *   Require 48MHz for USB OTG FS, : Enabled
+ *   SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 24MHz
+ * LSE - not installed
+ */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+//#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (8,000,000 / 8) * 336
+ *         = 336,000,000
+ * SYSCLK  = PLL_VCO / PLLP
+ *         = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ *         =  PLL_VCO / PLLQ
+ *         = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(24)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(336)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY  168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK  /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY  /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd4     /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLKd2     /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx.
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY   (2*STM32_PCLK1_FREQUENCY)
+
+/* LED definitions ******************************************************************/
+/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any
+ * way.  The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with stm32_setled() */
+
+#define BOARD_LED1        0
+#define BOARD_LED2        1
+#define BOARD_NLEDS       2
+
+#define BOARD_LED_BLUE    BOARD_LED1
+#define BOARD_LED_RED     BOARD_LED2
+
+/* LED bits for use with stm32_setleds() */
+
+#define BOARD_LED1_BIT    (1 << BOARD_LED1)
+#define BOARD_LED2_BIT    (1 << BOARD_LED2)
+
+/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the
+ * px4fmu-v1.  The following definitions describe how NuttX controls the LEDs:
+ */
+
+#define LED_STARTED       0  /* LED1 */
+#define LED_HEAPALLOCATE  1  /* LED2 */
+#define LED_IRQSENABLED   2  /* LED1 */
+#define LED_STACKCREATED  3  /* LED1 + LED2 */
+#define LED_INIRQ         4  /* LED1 */
+#define LED_SIGNAL        5  /* LED2 */
+#define LED_ASSERTION     6  /* LED1 + LED2 */
+#define LED_PANIC         7  /* LED1 + LED2 */
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ *
+ * Note that UART5 has no optional pinout, so it is not listed here.
+ */
+#define GPIO_USART1_RX	GPIO_USART1_RX_2
+#define GPIO_USART1_TX	GPIO_USART1_TX_2
+
+#define GPIO_USART2_RX	GPIO_USART2_RX_1
+#define GPIO_USART2_TX	GPIO_USART2_TX_1
+#define GPIO_USART2_RTS	GPIO_USART2_RTS_1
+#define GPIO_USART2_CTS	GPIO_USART2_CTS_1
+
+#define GPIO_USART6_RX	GPIO_USART6_RX_1
+#define GPIO_USART6_TX	GPIO_USART6_TX_1
+
+/* UART DMA configuration for USART1/6 */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+ 
+/*
+ * CAN
+ *
+ * CAN2 is routed to the expansion connector.
+ */
+
+#define GPIO_CAN2_RX	GPIO_CAN2_RX_2
+#define GPIO_CAN2_TX	GPIO_CAN2_TX_2
+
+/*
+ * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves.  They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+#define GPIO_I2C1_SCL		GPIO_I2C1_SCL_2
+#define GPIO_I2C1_SDA		GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+#define GPIO_I2C2_SCL		GPIO_I2C2_SCL_1
+#define GPIO_I2C2_SDA		GPIO_I2C2_SDA_1
+#define GPIO_I2C2_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+
+#define GPIO_I2C3_SCL		GPIO_I2C3_SCL_1
+#define GPIO_I2C3_SDA		GPIO_I2C3_SDA_1
+#define GPIO_I2C3_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_I2C3_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9)
+
+/*
+ * SPI
+ *
+ * There are sensors on SPI1, and SPI3 is connected to the microSD slot.
+ */
+#define GPIO_SPI1_MISO	GPIO_SPI1_MISO_1
+#define GPIO_SPI1_MOSI	GPIO_SPI1_MOSI_1
+#define GPIO_SPI1_SCK	GPIO_SPI1_SCK_1
+
+#define GPIO_SPI3_MISO	GPIO_SPI3_MISO_2
+#define GPIO_SPI3_MOSI	GPIO_SPI3_MOSI_1
+#define GPIO_SPI3_SCK	GPIO_SPI3_SCK_2
+#define GPIO_SPI3_NSS	GPIO_SPI3_NSS_2
+
+/* SPI DMA configuration for SPI3 (microSD) */
+#define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1
+#define DMACHAN_SPI3_TX DMAMAP_SPI3_TX_2
+/* XXX since we allocate the HP work stack from CCM RAM on normal system startup,
+   SPI1 will never run in DMA mode - so we can just give it a random config here.
+   What we really need to do is to make DMA configurable per channel, and always
+   disable it for SPI1. */
+#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
+#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+/************************************************************************************
+ * Name:  stm32_ledinit, stm32_setled, and stm32_setleds
+ *
+ * Description:
+ *   If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs.  If
+ *   CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to
+ *   control the LEDs from user applications.
+ *
+ ************************************************************************************/
+
+#ifndef CONFIG_ARCH_LEDS
+EXTERN void stm32_ledinit(void);
+EXTERN void stm32_setled(int led, bool ledon);
+EXTERN void stm32_setleds(uint8_t ledset);
+#endif
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */
diff --git a/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h
new file mode 100644
index 000000000..15e4e7a8d
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h
@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs
new file mode 100644
index 000000000..81936334b
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs
@@ -0,0 +1,3 @@
+include ${TOPDIR}/.config
+
+include $(TOPDIR)/configs/px4fmu-v1/common/Make.defs
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
new file mode 100644
index 000000000..eb225e22c
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -0,0 +1,957 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+# CONFIG_HOST_LINUX is not set
+CONFIG_HOST_OSX=y
+# CONFIG_HOST_WINDOWS is not set
+# CONFIG_HOST_OTHER is not set
+
+#
+# Build Configuration
+#
+CONFIG_APPS_DIR="../apps"
+# CONFIG_BUILD_2PASS is not set
+
+#
+# Binary Output Formats
+#
+# CONFIG_RRLOAD_BINARY is not set
+# CONFIG_INTELHEX_BINARY is not set
+# CONFIG_MOTOROLA_SREC is not set
+CONFIG_RAW_BINARY=y
+
+#
+# Customize Header Files
+#
+# CONFIG_ARCH_STDBOOL_H is not set
+CONFIG_ARCH_MATH_H=y
+# CONFIG_ARCH_FLOAT_H is not set
+# CONFIG_ARCH_STDARG_H is not set
+
+#
+# Debug Options
+#
+# CONFIG_DEBUG is not set
+CONFIG_DEBUG_SYMBOLS=y
+
+#
+# System Type
+#
+# CONFIG_ARCH_8051 is not set
+CONFIG_ARCH_ARM=y
+# CONFIG_ARCH_AVR is not set
+# CONFIG_ARCH_HC is not set
+# CONFIG_ARCH_MIPS is not set
+# CONFIG_ARCH_RGMP is not set
+# CONFIG_ARCH_SH is not set
+# CONFIG_ARCH_SIM is not set
+# CONFIG_ARCH_X86 is not set
+# CONFIG_ARCH_Z16 is not set
+# CONFIG_ARCH_Z80 is not set
+CONFIG_ARCH="arm"
+
+#
+# ARM Options
+#
+# CONFIG_ARCH_CHIP_C5471 is not set
+# CONFIG_ARCH_CHIP_CALYPSO is not set
+# CONFIG_ARCH_CHIP_DM320 is not set
+# CONFIG_ARCH_CHIP_IMX is not set
+# CONFIG_ARCH_CHIP_KINETIS is not set
+# CONFIG_ARCH_CHIP_KL is not set
+# CONFIG_ARCH_CHIP_LM is not set
+# CONFIG_ARCH_CHIP_LPC17XX is not set
+# CONFIG_ARCH_CHIP_LPC214X is not set
+# CONFIG_ARCH_CHIP_LPC2378 is not set
+# CONFIG_ARCH_CHIP_LPC31XX is not set
+# CONFIG_ARCH_CHIP_LPC43XX is not set
+# CONFIG_ARCH_CHIP_NUC1XX is not set
+# CONFIG_ARCH_CHIP_SAM3U is not set
+CONFIG_ARCH_CHIP_STM32=y
+# CONFIG_ARCH_CHIP_STR71X is not set
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_HAVE_FPU=y
+CONFIG_ARCH_FAMILY="armv7-m"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARCH_HAVE_CMNVECTOR=y
+CONFIG_ARMV7M_CMNVECTOR=y
+CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_ARCH_FPU=y
+CONFIG_ARCH_HAVE_MPU=y
+# CONFIG_ARMV7M_MPU is not set
+
+#
+# ARMV7M Configuration Options
+#
+# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SERIAL_DISABLE_REORDERING=y
+CONFIG_SERIAL_IFLOWCONTROL=y
+CONFIG_SERIAL_OFLOWCONTROL=y
+
+#
+# STM32 Configuration Options
+#
+# CONFIG_ARCH_CHIP_STM32L151C6 is not set
+# CONFIG_ARCH_CHIP_STM32L151C8 is not set
+# CONFIG_ARCH_CHIP_STM32L151CB is not set
+# CONFIG_ARCH_CHIP_STM32L151R6 is not set
+# CONFIG_ARCH_CHIP_STM32L151R8 is not set
+# CONFIG_ARCH_CHIP_STM32L151RB is not set
+# CONFIG_ARCH_CHIP_STM32L151V6 is not set
+# CONFIG_ARCH_CHIP_STM32L151V8 is not set
+# CONFIG_ARCH_CHIP_STM32L151VB is not set
+# CONFIG_ARCH_CHIP_STM32L152C6 is not set
+# CONFIG_ARCH_CHIP_STM32L152C8 is not set
+# CONFIG_ARCH_CHIP_STM32L152CB is not set
+# CONFIG_ARCH_CHIP_STM32L152R6 is not set
+# CONFIG_ARCH_CHIP_STM32L152R8 is not set
+# CONFIG_ARCH_CHIP_STM32L152RB is not set
+# CONFIG_ARCH_CHIP_STM32L152V6 is not set
+# CONFIG_ARCH_CHIP_STM32L152V8 is not set
+# CONFIG_ARCH_CHIP_STM32L152VB is not set
+# CONFIG_ARCH_CHIP_STM32F100C8 is not set
+# CONFIG_ARCH_CHIP_STM32F100CB is not set
+# CONFIG_ARCH_CHIP_STM32F100R8 is not set
+# CONFIG_ARCH_CHIP_STM32F100RB is not set
+# CONFIG_ARCH_CHIP_STM32F100RC is not set
+# CONFIG_ARCH_CHIP_STM32F100RD is not set
+# CONFIG_ARCH_CHIP_STM32F100RE is not set
+# CONFIG_ARCH_CHIP_STM32F100V8 is not set
+# CONFIG_ARCH_CHIP_STM32F100VB is not set
+# CONFIG_ARCH_CHIP_STM32F100VC is not set
+# CONFIG_ARCH_CHIP_STM32F100VD is not set
+# CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F103C4 is not set
+# CONFIG_ARCH_CHIP_STM32F103C8 is not set
+# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
+# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
+# CONFIG_ARCH_CHIP_STM32F107VC is not set
+# CONFIG_ARCH_CHIP_STM32F207IG is not set
+# CONFIG_ARCH_CHIP_STM32F302CB is not set
+# CONFIG_ARCH_CHIP_STM32F302CC is not set
+# CONFIG_ARCH_CHIP_STM32F302RB is not set
+# CONFIG_ARCH_CHIP_STM32F302RC is not set
+# CONFIG_ARCH_CHIP_STM32F302VB is not set
+# CONFIG_ARCH_CHIP_STM32F302VC is not set
+# CONFIG_ARCH_CHIP_STM32F303CB is not set
+# CONFIG_ARCH_CHIP_STM32F303CC is not set
+# CONFIG_ARCH_CHIP_STM32F303RB is not set
+# CONFIG_ARCH_CHIP_STM32F303RC is not set
+# CONFIG_ARCH_CHIP_STM32F303VB is not set
+# CONFIG_ARCH_CHIP_STM32F303VC is not set
+CONFIG_ARCH_CHIP_STM32F405RG=y
+# CONFIG_ARCH_CHIP_STM32F405VG is not set
+# CONFIG_ARCH_CHIP_STM32F405ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407VE is not set
+# CONFIG_ARCH_CHIP_STM32F407VG is not set
+# CONFIG_ARCH_CHIP_STM32F407ZE is not set
+# CONFIG_ARCH_CHIP_STM32F407ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407IE is not set
+# CONFIG_ARCH_CHIP_STM32F407IG is not set
+# CONFIG_ARCH_CHIP_STM32F427V is not set
+# CONFIG_ARCH_CHIP_STM32F427Z is not set
+# CONFIG_ARCH_CHIP_STM32F427I is not set
+# CONFIG_STM32_STM32L15XX is not set
+# CONFIG_STM32_ENERGYLITE is not set
+# CONFIG_STM32_STM32F10XX is not set
+# CONFIG_STM32_VALUELINE is not set
+# CONFIG_STM32_CONNECTIVITYLINE is not set
+# CONFIG_STM32_PERFORMANCELINE is not set
+# CONFIG_STM32_HIGHDENSITY is not set
+# CONFIG_STM32_MEDIUMDENSITY is not set
+# CONFIG_STM32_LOWDENSITY is not set
+# CONFIG_STM32_STM32F20XX is not set
+# CONFIG_STM32_STM32F30XX is not set
+CONFIG_STM32_STM32F40XX=y
+# CONFIG_STM32_DFU is not set
+
+#
+# STM32 Peripheral Support
+#
+CONFIG_STM32_ADC1=y
+# CONFIG_STM32_ADC2 is not set
+# CONFIG_STM32_ADC3 is not set
+CONFIG_STM32_BKPSRAM=y
+# CONFIG_STM32_CAN1 is not set
+# CONFIG_STM32_CAN2 is not set
+CONFIG_STM32_CCMDATARAM=y
+# CONFIG_STM32_CRC is not set
+# CONFIG_STM32_CRYP is not set
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+# CONFIG_STM32_DAC1 is not set
+# CONFIG_STM32_DAC2 is not set
+# CONFIG_STM32_DCMI is not set
+# CONFIG_STM32_ETHMAC is not set
+# CONFIG_STM32_FSMC is not set
+# CONFIG_STM32_HASH is not set
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_I2C2=y
+CONFIG_STM32_I2C3=y
+CONFIG_STM32_OTGFS=y
+# CONFIG_STM32_OTGHS is not set
+CONFIG_STM32_PWR=y
+# CONFIG_STM32_RNG is not set
+# CONFIG_STM32_SDIO is not set
+CONFIG_STM32_SPI1=y
+# CONFIG_STM32_SPI2 is not set
+CONFIG_STM32_SPI3=y
+CONFIG_STM32_SYSCFG=y
+# CONFIG_STM32_TIM1 is not set
+# CONFIG_STM32_TIM2 is not set
+# CONFIG_STM32_TIM3 is not set
+CONFIG_STM32_TIM4=y
+CONFIG_STM32_TIM5=y
+CONFIG_STM32_TIM6=y
+CONFIG_STM32_TIM7=y
+# CONFIG_STM32_TIM8 is not set
+CONFIG_STM32_TIM9=y
+CONFIG_STM32_TIM10=y
+CONFIG_STM32_TIM11=y
+CONFIG_STM32_TIM12=y
+CONFIG_STM32_TIM13=y
+CONFIG_STM32_TIM14=y
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+# CONFIG_STM32_USART3 is not set
+# CONFIG_STM32_UART4 is not set
+CONFIG_STM32_UART5=y
+CONFIG_STM32_USART6=y
+# CONFIG_STM32_IWDG is not set
+CONFIG_STM32_WWDG=y
+CONFIG_STM32_ADC=y
+CONFIG_STM32_SPI=y
+CONFIG_STM32_I2C=y
+
+#
+# Alternate Pin Mapping
+#
+CONFIG_STM32_FLASH_PREFETCH=y
+# CONFIG_STM32_JTAG_DISABLE is not set
+CONFIG_STM32_JTAG_FULL_ENABLE=y
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+# CONFIG_STM32_JTAG_SW_ENABLE is not set
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+# CONFIG_STM32_FORCEPOWER is not set
+# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
+# CONFIG_STM32_CCMEXCLUDE is not set
+# CONFIG_STM32_TIM4_PWM is not set
+# CONFIG_STM32_TIM5_PWM is not set
+# CONFIG_STM32_TIM9_PWM is not set
+# CONFIG_STM32_TIM10_PWM is not set
+# CONFIG_STM32_TIM11_PWM is not set
+# CONFIG_STM32_TIM12_PWM is not set
+# CONFIG_STM32_TIM13_PWM is not set
+# CONFIG_STM32_TIM14_PWM is not set
+# CONFIG_STM32_TIM4_ADC is not set
+# CONFIG_STM32_TIM5_ADC is not set
+CONFIG_STM32_USART=y
+
+#
+# U[S]ART Configuration
+#
+# CONFIG_USART1_RS485 is not set
+CONFIG_USART1_RXDMA=y
+# CONFIG_USART2_RS485 is not set
+CONFIG_USART2_RXDMA=y
+# CONFIG_USART3_RXDMA is not set
+# CONFIG_UART4_RXDMA is not set
+# CONFIG_UART5_RS485 is not set
+CONFIG_UART5_RXDMA=y
+# CONFIG_USART6_RS485 is not set
+CONFIG_USART6_RXDMA=y
+# CONFIG_USART7_RXDMA is not set
+# CONFIG_USART8_RXDMA is not set
+CONFIG_STM32_USART_SINGLEWIRE=y
+
+#
+# SPI Configuration
+#
+# CONFIG_STM32_SPI_INTERRUPTS is not set
+# CONFIG_STM32_SPI_DMA is not set
+
+#
+# I2C Configuration
+#
+# CONFIG_STM32_I2C_DYNTIMEO is not set
+CONFIG_STM32_I2CTIMEOSEC=0
+CONFIG_STM32_I2CTIMEOMS=10
+CONFIG_STM32_I2CTIMEOTICKS=500
+# CONFIG_STM32_I2C_DUTY16_9 is not set
+
+#
+# USB Host Configuration
+#
+
+#
+# USB Device Configuration
+#
+
+#
+# External Memory Configuration
+#
+
+#
+# Architecture Options
+#
+# CONFIG_ARCH_NOINTC is not set
+# CONFIG_ARCH_VECNOTIRQ is not set
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_IRQPRIO=y
+# CONFIG_CUSTOM_STACK is not set
+# CONFIG_ADDRENV is not set
+CONFIG_ARCH_HAVE_VFORK=y
+CONFIG_ARCH_STACKDUMP=y
+# CONFIG_ENDIAN_BIG is not set
+# CONFIG_ARCH_HAVE_RAMFUNCS is not set
+CONFIG_ARCH_HAVE_RAMVECTORS=y
+# CONFIG_ARCH_RAMVECTORS is not set
+
+#
+# Board Settings
+#
+CONFIG_BOARD_LOOPSPERMSEC=16717
+# CONFIG_ARCH_CALIBRATION is not set
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_SIZE=196608
+CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
+CONFIG_ARCH_INTERRUPTSTACK=0
+
+#
+# Boot options
+#
+# CONFIG_BOOT_RUNFROMEXTSRAM is not set
+CONFIG_BOOT_RUNFROMFLASH=y
+# CONFIG_BOOT_RUNFROMISRAM is not set
+# CONFIG_BOOT_RUNFROMSDRAM is not set
+# CONFIG_BOOT_COPYTORAM is not set
+
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_PX4FMU_V1=y
+# CONFIG_ARCH_BOARD_CUSTOM is not set
+CONFIG_ARCH_BOARD="px4fmu-v1"
+
+#
+# Common Board Options
+#
+CONFIG_ARCH_HAVE_LEDS=y
+# CONFIG_ARCH_LEDS is not set
+CONFIG_NSH_MMCSDMINOR=0
+CONFIG_NSH_MMCSDSLOTNO=0
+CONFIG_NSH_MMCSDSPIPORTNO=3
+
+#
+# Board-Specific Options
+#
+CONFIG_HRT_TIMER=y
+CONFIG_HRT_PPM=y
+
+#
+# RTOS Features
+#
+# CONFIG_BOARD_INITIALIZE is not set
+CONFIG_MSEC_PER_TICK=1
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_TASK_NAME_SIZE=24
+# CONFIG_SCHED_HAVE_PARENT is not set
+# CONFIG_JULIAN_TIME is not set
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+# CONFIG_DEV_CONSOLE is not set
+# CONFIG_MUTEX_TYPES is not set
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=8
+# CONFIG_FDCLONE_DISABLE is not set
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WAITPID=y
+# CONFIG_SCHED_STARTHOOK is not set
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_ATEXIT_MAX=1
+# CONFIG_SCHED_ONEXIT is not set
+CONFIG_USER_ENTRYPOINT="nsh_main"
+CONFIG_DISABLE_OS_API=y
+# CONFIG_DISABLE_CLOCK is not set
+# CONFIG_DISABLE_POSIX_TIMERS is not set
+# CONFIG_DISABLE_PTHREAD is not set
+# CONFIG_DISABLE_SIGNALS is not set
+# CONFIG_DISABLE_MQUEUE is not set
+# CONFIG_DISABLE_ENVIRON is not set
+
+#
+# Signal Numbers
+#
+CONFIG_SIG_SIGUSR1=1
+CONFIG_SIG_SIGUSR2=2
+CONFIG_SIG_SIGALARM=3
+CONFIG_SIG_SIGCONDTIMEDOUT=16
+CONFIG_SIG_SIGWORK=4
+
+#
+# Sizes of configurable things (0 disables)
+#
+CONFIG_MAX_TASKS=32
+CONFIG_MAX_TASK_ARGS=10
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_STREAMS=25
+CONFIG_NAME_MAX=32
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PREALLOC_TIMERS=50
+
+#
+# Stack and heap information
+#
+CONFIG_IDLETHREAD_STACKSIZE=6000
+CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+
+#
+# Device Drivers
+#
+# CONFIG_DISABLE_POLL is not set
+CONFIG_DEV_NULL=y
+# CONFIG_DEV_ZERO is not set
+# CONFIG_LOOP is not set
+# CONFIG_RAMDISK is not set
+# CONFIG_CAN is not set
+# CONFIG_PWM is not set
+CONFIG_I2C=y
+# CONFIG_I2C_SLAVE is not set
+CONFIG_I2C_TRANSFER=y
+# CONFIG_I2C_WRITEREAD is not set
+# CONFIG_I2C_POLLED is not set
+# CONFIG_I2C_TRACE is not set
+CONFIG_ARCH_HAVE_I2CRESET=y
+CONFIG_I2C_RESET=y
+CONFIG_SPI=y
+# CONFIG_SPI_OWNBUS is not set
+CONFIG_SPI_EXCHANGE=y
+# CONFIG_SPI_CMDDATA is not set
+# CONFIG_RTC is not set
+CONFIG_WATCHDOG=y
+# CONFIG_ANALOG is not set
+# CONFIG_AUDIO_DEVICES is not set
+# CONFIG_BCH is not set
+# CONFIG_INPUT is not set
+# CONFIG_LCD is not set
+CONFIG_MMCSD=y
+CONFIG_MMCSD_NSLOTS=1
+# CONFIG_MMCSD_READONLY is not set
+# CONFIG_MMCSD_MULTIBLOCK_DISABLE is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_HAVECARDDETECT is not set
+CONFIG_MMCSD_SPI=y
+CONFIG_MMCSD_SPICLOCK=24000000
+# CONFIG_MMCSD_SDIO is not set
+# CONFIG_MTD is not set
+CONFIG_PIPES=y
+# CONFIG_PM is not set
+# CONFIG_POWER is not set
+# CONFIG_SENSORS is not set
+CONFIG_SERIAL=y
+# CONFIG_DEV_LOWCONSOLE is not set
+CONFIG_SERIAL_REMOVABLE=y
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_UART5=y
+CONFIG_ARCH_HAVE_USART1=y
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_ARCH_HAVE_USART6=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+# CONFIG_USART1_SERIAL_CONSOLE is not set
+# CONFIG_USART2_SERIAL_CONSOLE is not set
+# CONFIG_UART5_SERIAL_CONSOLE is not set
+# CONFIG_USART6_SERIAL_CONSOLE is not set
+CONFIG_NO_SERIAL_CONSOLE=y
+
+#
+# USART1 Configuration
+#
+CONFIG_USART1_RXBUFSIZE=512
+CONFIG_USART1_TXBUFSIZE=512
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_BITS=8
+CONFIG_USART1_PARITY=0
+CONFIG_USART1_2STOP=0
+
+#
+# USART2 Configuration
+#
+CONFIG_USART2_RXBUFSIZE=512
+CONFIG_USART2_TXBUFSIZE=512
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+
+#
+# UART5 Configuration
+#
+CONFIG_UART5_RXBUFSIZE=32
+CONFIG_UART5_TXBUFSIZE=32
+CONFIG_UART5_BAUD=57600
+CONFIG_UART5_BITS=8
+CONFIG_UART5_PARITY=0
+CONFIG_UART5_2STOP=0
+
+#
+# USART6 Configuration
+#
+CONFIG_USART6_RXBUFSIZE=512
+CONFIG_USART6_TXBUFSIZE=512
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_BITS=8
+CONFIG_USART6_PARITY=0
+CONFIG_USART6_2STOP=0
+CONFIG_USBDEV=y
+
+#
+# USB Device Controller Driver Options
+#
+# CONFIG_USBDEV_ISOCHRONOUS is not set
+# CONFIG_USBDEV_DUALSPEED is not set
+CONFIG_USBDEV_SELFPOWERED=y
+# CONFIG_USBDEV_BUSPOWERED is not set
+CONFIG_USBDEV_MAXPOWER=500
+# CONFIG_USBDEV_DMA is not set
+# CONFIG_USBDEV_TRACE is not set
+
+#
+# USB Device Class Driver Options
+#
+# CONFIG_USBDEV_COMPOSITE is not set
+# CONFIG_PL2303 is not set
+CONFIG_CDCACM=y
+CONFIG_CDCACM_CONSOLE=y
+CONFIG_CDCACM_EP0MAXPACKET=64
+CONFIG_CDCACM_EPINTIN=1
+CONFIG_CDCACM_EPINTIN_FSSIZE=64
+CONFIG_CDCACM_EPINTIN_HSSIZE=64
+CONFIG_CDCACM_EPBULKOUT=3
+CONFIG_CDCACM_EPBULKOUT_FSSIZE=64
+CONFIG_CDCACM_EPBULKOUT_HSSIZE=512
+CONFIG_CDCACM_EPBULKIN=2
+CONFIG_CDCACM_EPBULKIN_FSSIZE=64
+CONFIG_CDCACM_EPBULKIN_HSSIZE=512
+CONFIG_CDCACM_NWRREQS=4
+CONFIG_CDCACM_NRDREQS=4
+CONFIG_CDCACM_RXBUFSIZE=256
+CONFIG_CDCACM_TXBUFSIZE=256
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_PRODUCTID=0x0010
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.x"
+# CONFIG_USBMSC is not set
+# CONFIG_USBHOST is not set
+# CONFIG_WIRELESS is not set
+
+#
+# System Logging Device Options
+#
+
+#
+# System Logging
+#
+# CONFIG_RAMLOG is not set
+
+#
+# Networking Support
+#
+# CONFIG_NET is not set
+
+#
+# File Systems
+#
+
+#
+# File system configuration
+#
+# CONFIG_DISABLE_MOUNTPOINT is not set
+# CONFIG_FS_RAMMAP is not set
+CONFIG_FS_FAT=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_MAXFNAME=32
+CONFIG_FS_FATTIME=y
+# CONFIG_FAT_DMAMEMORY is not set
+CONFIG_FS_NXFFS=y
+CONFIG_NXFFS_PREALLOCATED=y
+CONFIG_NXFFS_ERASEDSTATE=0xff
+CONFIG_NXFFS_PACKTHRESHOLD=32
+CONFIG_NXFFS_MAXNAMLEN=32
+CONFIG_NXFFS_TAILTHRESHOLD=2048
+CONFIG_FS_ROMFS=y
+# CONFIG_FS_SMARTFS is not set
+CONFIG_FS_BINFS=y
+
+#
+# System Logging
+#
+# CONFIG_SYSLOG_ENABLE is not set
+CONFIG_SYSLOG=y
+CONFIG_SYSLOG_CHAR=y
+CONFIG_SYSLOG_DEVPATH="/dev/ttyS0"
+
+#
+# Graphics Support
+#
+# CONFIG_NX is not set
+
+#
+# Memory Management
+#
+# CONFIG_MM_MULTIHEAP is not set
+# CONFIG_MM_SMALL is not set
+CONFIG_MM_REGIONS=2
+CONFIG_GRAN=y
+CONFIG_GRAN_SINGLE=y
+CONFIG_GRAN_INTR=y
+
+#
+# Audio Support
+#
+# CONFIG_AUDIO is not set
+
+#
+# Binary Formats
+#
+# CONFIG_BINFMT_DISABLE is not set
+# CONFIG_BINFMT_EXEPATH is not set
+# CONFIG_NXFLAT is not set
+# CONFIG_ELF is not set
+CONFIG_BUILTIN=y
+# CONFIG_PIC is not set
+# CONFIG_SYMTAB_ORDEREDBYNAME is not set
+
+#
+# Library Routines
+#
+
+#
+# Standard C Library Options
+#
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+CONFIG_LIB_HOMEDIR="/"
+# CONFIG_NOPRINTF_FIELDWIDTH is not set
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIB_RAND_ORDER=1
+# CONFIG_EOL_IS_CR is not set
+# CONFIG_EOL_IS_LF is not set
+# CONFIG_EOL_IS_BOTH_CRLF is not set
+CONFIG_EOL_IS_EITHER_CRLF=y
+# CONFIG_LIBC_EXECFUNCS is not set
+CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024
+CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048
+CONFIG_LIBC_STRERROR=y
+# CONFIG_LIBC_STRERROR_SHORT is not set
+# CONFIG_LIBC_PERROR_STDOUT is not set
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_LIB_SENDFILE_BUFSIZE=512
+# CONFIG_ARCH_ROMGETC is not set
+CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y
+CONFIG_ARCH_MEMCPY=y
+# CONFIG_ARCH_MEMCMP is not set
+# CONFIG_ARCH_MEMMOVE is not set
+# CONFIG_ARCH_MEMSET is not set
+# CONFIG_MEMSET_OPTSPEED is not set
+# CONFIG_ARCH_STRCHR is not set
+# CONFIG_ARCH_STRCMP is not set
+# CONFIG_ARCH_STRCPY is not set
+# CONFIG_ARCH_STRNCPY is not set
+# CONFIG_ARCH_STRLEN is not set
+# CONFIG_ARCH_STRNLEN is not set
+# CONFIG_ARCH_BZERO is not set
+
+#
+# Non-standard Library Support
+#
+CONFIG_SCHED_WORKQUEUE=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_WORKPRIORITY=192
+CONFIG_SCHED_WORKPERIOD=5000
+CONFIG_SCHED_WORKSTACKSIZE=2048
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=2048
+# CONFIG_LIB_KBDCODEC is not set
+# CONFIG_LIB_SLCDCODEC is not set
+
+#
+# Basic CXX Support
+#
+# CONFIG_C99_BOOL8 is not set
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+# CONFIG_CXX_NEWLONG is not set
+
+#
+# uClibc++ Standard C++ Library
+#
+# CONFIG_UCLIBCXX is not set
+
+#
+# Application Configuration
+#
+
+#
+# Built-In Applications
+#
+CONFIG_BUILTIN_PROXY_STACKSIZE=1024
+
+#
+# Examples
+#
+# CONFIG_EXAMPLES_BUTTONS is not set
+# CONFIG_EXAMPLES_CAN is not set
+CONFIG_EXAMPLES_CDCACM=y
+# CONFIG_EXAMPLES_COMPOSITE is not set
+# CONFIG_EXAMPLES_CXXTEST is not set
+# CONFIG_EXAMPLES_DHCPD is not set
+# CONFIG_EXAMPLES_ELF is not set
+# CONFIG_EXAMPLES_FTPC is not set
+# CONFIG_EXAMPLES_FTPD is not set
+# CONFIG_EXAMPLES_HELLO is not set
+# CONFIG_EXAMPLES_HELLOXX is not set
+# CONFIG_EXAMPLES_JSON is not set
+# CONFIG_EXAMPLES_HIDKBD is not set
+# CONFIG_EXAMPLES_KEYPADTEST is not set
+# CONFIG_EXAMPLES_IGMP is not set
+# CONFIG_EXAMPLES_LCDRW is not set
+# CONFIG_EXAMPLES_MM is not set
+# CONFIG_EXAMPLES_MODBUS is not set
+CONFIG_EXAMPLES_MOUNT=y
+CONFIG_EXAMPLES_NSH=y
+# CONFIG_EXAMPLES_NULL is not set
+# CONFIG_EXAMPLES_NX is not set
+# CONFIG_EXAMPLES_NXCONSOLE is not set
+# CONFIG_EXAMPLES_NXFFS is not set
+# CONFIG_EXAMPLES_NXFLAT is not set
+# CONFIG_EXAMPLES_NXHELLO is not set
+# CONFIG_EXAMPLES_NXIMAGE is not set
+# CONFIG_EXAMPLES_NXLINES is not set
+# CONFIG_EXAMPLES_NXTEXT is not set
+# CONFIG_EXAMPLES_OSTEST is not set
+# CONFIG_EXAMPLES_PASHELLO is not set
+# CONFIG_EXAMPLES_PIPE is not set
+# CONFIG_EXAMPLES_POSIXSPAWN is not set
+# CONFIG_EXAMPLES_QENCODER is not set
+# CONFIG_EXAMPLES_RGMP is not set
+# CONFIG_EXAMPLES_ROMFS is not set
+# CONFIG_EXAMPLES_SENDMAIL is not set
+# CONFIG_EXAMPLES_SERLOOP is not set
+# CONFIG_EXAMPLES_SLCD is not set
+# CONFIG_EXAMPLES_SMART_TEST is not set
+# CONFIG_EXAMPLES_SMART is not set
+# CONFIG_EXAMPLES_TCPECHO is not set
+# CONFIG_EXAMPLES_TELNETD is not set
+# CONFIG_EXAMPLES_THTTPD is not set
+# CONFIG_EXAMPLES_TIFF is not set
+# CONFIG_EXAMPLES_TOUCHSCREEN is not set
+# CONFIG_EXAMPLES_UDP is not set
+# CONFIG_EXAMPLES_UIP is not set
+# CONFIG_EXAMPLES_USBSERIAL is not set
+# CONFIG_EXAMPLES_USBMSC is not set
+# CONFIG_EXAMPLES_USBTERM is not set
+# CONFIG_EXAMPLES_WATCHDOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_TIFF is not set
+
+#
+# Interpreters
+#
+# CONFIG_INTERPRETERS_FICL is not set
+# CONFIG_INTERPRETERS_PCODE is not set
+
+#
+# Network Utilities
+#
+
+#
+# Networking Utilities
+#
+# CONFIG_NETUTILS_CODECS is not set
+# CONFIG_NETUTILS_DHCPC is not set
+# CONFIG_NETUTILS_DHCPD is not set
+# CONFIG_NETUTILS_FTPC is not set
+# CONFIG_NETUTILS_FTPD is not set
+# CONFIG_NETUTILS_JSON is not set
+# CONFIG_NETUTILS_RESOLV is not set
+# CONFIG_NETUTILS_SMTP is not set
+# CONFIG_NETUTILS_TELNETD is not set
+# CONFIG_NETUTILS_TFTPC is not set
+# CONFIG_NETUTILS_THTTPD is not set
+# CONFIG_NETUTILS_UIPLIB is not set
+# CONFIG_NETUTILS_WEBCLIENT is not set
+
+#
+# FreeModBus
+#
+# CONFIG_MODBUS is not set
+
+#
+# NSH Library
+#
+CONFIG_NSH_LIBRARY=y
+CONFIG_NSH_BUILTIN_APPS=y
+
+#
+# Disable Individual commands
+#
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DD is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_HEXDUMP is not set
+# CONFIG_NSH_DISABLE_IFCONFIG is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOSETUP is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MB is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MKFIFO is not set
+# CONFIG_NSH_DISABLE_MKRD is not set
+# CONFIG_NSH_DISABLE_MH is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MW is not set
+# CONFIG_NSH_DISABLE_NSFMOUNT is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PING is not set
+# CONFIG_NSH_DISABLE_PUT is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SH is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+# CONFIG_NSH_DISABLE_WGET is not set
+# CONFIG_NSH_DISABLE_XD is not set
+
+#
+# Configure Command Options
+#
+# CONFIG_NSH_CMDOPT_DF_H is not set
+CONFIG_NSH_CODECS_BUFSIZE=128
+CONFIG_NSH_FILEIOSIZE=512
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=12
+CONFIG_NSH_NESTDEPTH=8
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLEBG is not set
+CONFIG_NSH_ROMFSETC=y
+# CONFIG_NSH_ROMFSRC is not set
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_FATDEVNO=1
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT="/tmp"
+CONFIG_NSH_CONSOLE=y
+# CONFIG_NSH_USBCONSOLE is not set
+
+#
+# USB Trace Support
+#
+# CONFIG_NSH_CONDEV is not set
+CONFIG_NSH_ARCHINIT=y
+
+#
+# NxWidgets/NxWM
+#
+
+#
+# System NSH Add-Ons
+#
+
+#
+# Custom Free Memory Command
+#
+# CONFIG_SYSTEM_FREE is not set
+
+#
+# I2C tool
+#
+# CONFIG_SYSTEM_I2CTOOL is not set
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# FLASH Erase-all Command
+#
+
+#
+# readline()
+#
+CONFIG_SYSTEM_READLINE=y
+CONFIG_READLINE_ECHO=y
+
+#
+# Power Off
+#
+# CONFIG_SYSTEM_POWEROFF is not set
+
+#
+# RAMTRON
+#
+# CONFIG_SYSTEM_RAMTRON is not set
+
+#
+# SD Card
+#
+# CONFIG_SYSTEM_SDCARD is not set
+
+#
+# Sysinfo
+#
+CONFIG_SYSTEM_SYSINFO=y
+
+#
+# USB Monitor
+#
diff --git a/nuttx-configs/px4fmu-v1/nsh/setenv.sh b/nuttx-configs/px4fmu-v1/nsh/setenv.sh
new file mode 100755
index 000000000..db372217c
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/nsh/setenv.sh
@@ -0,0 +1,75 @@
+#!/bin/bash
+# configs/px4fmu-v1/usbnsh/setenv.sh
+#
+#   Copyright (C) 2013 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$_" = "$0" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+  echo "This script must be executed from the top-level NuttX build directory"
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+  export PATH_ORIG="${PATH}"
+fi
+
+# This is the Cygwin path to the location where I installed the RIDE
+# toolchain under windows.  You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This is the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows.  You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# These are the Cygwin paths to the locations where I installed the Atollic
+# toolchain under windows.  You will also have to edit this if you install
+# the Atollic toolchain in any other location.  /usr/bin is added before
+# the Atollic bin path because there is are binaries named gcc.exe and g++.exe
+# at those locations as well.
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin"
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin"
+
+# This is the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4fmu-v1/ostest/Make.defs b/nuttx-configs/px4fmu-v1/ostest/Make.defs
new file mode 100644
index 000000000..7b807abdb
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/ostest/Make.defs
@@ -0,0 +1,122 @@
+############################################################################
+# configs/stm32f4discovery/ostest/Make.defs
+#
+#   Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+LDSCRIPT = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+  MAXOPTIMIZATION = -O2
+else
+  # Linux/Cygwin-native toolchain
+  ARCHINCLUDES = -I. -isystem $(TOPDIR)/include
+  ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+  ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+endif
+
+CC = $(CROSSDEV)gcc
+CXX = $(CROSSDEV)g++
+CPP = $(CROSSDEV)gcc -E
+LD = $(CROSSDEV)ld
+AR = $(ARCROSSDEV)ar rcs
+NM = $(ARCROSSDEV)nm
+OBJCOPY = $(CROSSDEV)objcopy
+OBJDUMP = $(CROSSDEV)objdump
+
+ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+ifeq ($(CONFIG_DEBUG_SYMBOLS),y)
+  ARCHOPTIMIZATION = -g
+else
+  ARCHOPTIMIZATION = $(MAXOPTIMIZATION) -fno-strict-aliasing -fno-strength-reduce -fomit-frame-pointer
+endif
+
+ARCHCFLAGS = -fno-builtin
+ARCHCXXFLAGS = -fno-builtin -fno-exceptions -fno-rtti
+ARCHWARNINGS = -Wall -Wstrict-prototypes -Wshadow
+ARCHWARNINGSXX = -Wall -Wshadow
+ARCHDEFINES =
+ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10
+
+CFLAGS = $(ARCHCFLAGS) $(ARCHWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS = $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1 = -r -d -warn-common
+NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat-pcrel.ld -no-check-sections
+LDNXFLATFLAGS = -e main -s 2048
+
+OBJEXT = .o
+LIBEXT = .a
+EXEEXT =
+
+ifneq ($(CROSSDEV),arm-nuttx-elf-)
+  LDFLAGS += -nostartfiles -nodefaultlibs
+endif
+ifeq ($(CONFIG_DEBUG_SYMBOLS),y)
+  LDFLAGS += -g
+endif
+
+
+HOSTCC = gcc
+HOSTINCLUDES = -I.
+HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS =
+ifeq ($(CONFIG_HOST_WINDOWS),y)
+  HOSTEXEEXT = .exe
+else
+  HOSTEXEEXT =
+endif
+
+ifeq ($(WINTOOL),y)
+  # Windows-native host tools
+  DIRLINK = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK = $(TOPDIR)/tools/unlink.sh
+  MKDEP = $(TOPDIR)/tools/mknulldeps.sh
+else
+  # Linux/Cygwin-native host tools 
+  MKDEP = $(TOPDIR)/tools/mkdeps$(HOSTEXEEXT)
+endif
+
diff --git a/nuttx-configs/px4fmu-v1/ostest/defconfig b/nuttx-configs/px4fmu-v1/ostest/defconfig
new file mode 100644
index 000000000..c7fb6b2a5
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/ostest/defconfig
@@ -0,0 +1,583 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+# CONFIG_HOST_LINUX is not set
+# CONFIG_HOST_OSX is not set
+CONFIG_HOST_WINDOWS=y
+# CONFIG_HOST_OTHER is not set
+# CONFIG_WINDOWS_NATIVE is not set
+CONFIG_WINDOWS_CYGWIN=y
+# CONFIG_WINDOWS_MSYS is not set
+# CONFIG_WINDOWS_OTHER is not set
+# CONFIG_WINDOWS_MKLINK is not set
+
+#
+# Build Configuration
+#
+# CONFIG_APPS_DIR="../apps"
+# CONFIG_BUILD_2PASS is not set
+
+#
+# Binary Output Formats
+#
+# CONFIG_RRLOAD_BINARY is not set
+CONFIG_INTELHEX_BINARY=y
+# CONFIG_MOTOROLA_SREC is not set
+CONFIG_RAW_BINARY=y
+
+#
+# Customize Header Files
+#
+# CONFIG_ARCH_STDBOOL_H is not set
+# CONFIG_ARCH_MATH_H is not set
+# CONFIG_ARCH_FLOAT_H is not set
+# CONFIG_ARCH_STDARG_H is not set
+
+#
+# Debug Options
+#
+# CONFIG_DEBUG is not set
+# CONFIG_DEBUG_SYMBOLS is not set
+
+#
+# System Type
+#
+# CONFIG_ARCH_8051 is not set
+CONFIG_ARCH_ARM=y
+# CONFIG_ARCH_AVR is not set
+# CONFIG_ARCH_HC is not set
+# CONFIG_ARCH_MIPS is not set
+# CONFIG_ARCH_RGMP is not set
+# CONFIG_ARCH_SH is not set
+# CONFIG_ARCH_SIM is not set
+# CONFIG_ARCH_X86 is not set
+# CONFIG_ARCH_Z16 is not set
+# CONFIG_ARCH_Z80 is not set
+CONFIG_ARCH="arm"
+
+#
+# ARM Options
+#
+# CONFIG_ARCH_CHIP_C5471 is not set
+# CONFIG_ARCH_CHIP_CALYPSO is not set
+# CONFIG_ARCH_CHIP_DM320 is not set
+# CONFIG_ARCH_CHIP_IMX is not set
+# CONFIG_ARCH_CHIP_KINETIS is not set
+# CONFIG_ARCH_CHIP_LM3S is not set
+# CONFIG_ARCH_CHIP_LPC17XX is not set
+# CONFIG_ARCH_CHIP_LPC214X is not set
+# CONFIG_ARCH_CHIP_LPC2378 is not set
+# CONFIG_ARCH_CHIP_LPC31XX is not set
+# CONFIG_ARCH_CHIP_LPC43XX is not set
+# CONFIG_ARCH_CHIP_SAM3U is not set
+CONFIG_ARCH_CHIP_STM32=y
+# CONFIG_ARCH_CHIP_STR71X is not set
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_FAMILY="armv7-m"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_HAVE_CMNVECTOR=y
+# CONFIG_ARMV7M_CMNVECTOR is not set
+# CONFIG_ARCH_FPU is not set
+CONFIG_ARCH_HAVE_MPU=y
+# CONFIG_ARMV7M_MPU is not set
+CONFIG_ARCH_IRQPRIO=y
+CONFIG_BOARD_LOOPSPERMSEC=16717
+# CONFIG_ARCH_CALIBRATION is not set
+# CONFIG_SERIAL_TERMIOS is not set
+
+#
+# STM32 Configuration Options
+#
+# CONFIG_ARCH_CHIP_STM32F100C8 is not set
+# CONFIG_ARCH_CHIP_STM32F100CB is not set
+# CONFIG_ARCH_CHIP_STM32F100R8 is not set
+# CONFIG_ARCH_CHIP_STM32F100RB is not set
+# CONFIG_ARCH_CHIP_STM32F100RC is not set
+# CONFIG_ARCH_CHIP_STM32F100RD is not set
+# CONFIG_ARCH_CHIP_STM32F100RE is not set
+# CONFIG_ARCH_CHIP_STM32F100V8 is not set
+# CONFIG_ARCH_CHIP_STM32F100VB is not set
+# CONFIG_ARCH_CHIP_STM32F100VC is not set
+# CONFIG_ARCH_CHIP_STM32F100VD is not set
+# CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
+# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
+# CONFIG_ARCH_CHIP_STM32F107VC is not set
+# CONFIG_ARCH_CHIP_STM32F207IG is not set
+# CONFIG_ARCH_CHIP_STM32F405RG is not set
+# CONFIG_ARCH_CHIP_STM32F405VG is not set
+# CONFIG_ARCH_CHIP_STM32F405ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407VE is not set
+CONFIG_ARCH_CHIP_STM32F407VG=y
+# CONFIG_ARCH_CHIP_STM32F407ZE is not set
+# CONFIG_ARCH_CHIP_STM32F407ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407IE is not set
+# CONFIG_ARCH_CHIP_STM32F407IG is not set
+CONFIG_STM32_STM32F40XX=y
+# CONFIG_STM32_CODESOURCERYW is not set
+CONFIG_STM32_CODESOURCERYL=y
+# CONFIG_STM32_ATOLLIC_LITE is not set
+# CONFIG_STM32_ATOLLIC_PRO is not set
+# CONFIG_STM32_DEVKITARM is not set
+# CONFIG_STM32_RAISONANCE is not set
+# CONFIG_STM32_BUILDROOT is not set
+# CONFIG_STM32_DFU is not set
+
+#
+# STM32 Peripheral Support
+#
+# CONFIG_STM32_ADC1 is not set
+# CONFIG_STM32_ADC2 is not set
+# CONFIG_STM32_ADC3 is not set
+# CONFIG_STM32_BKPSRAM is not set
+# CONFIG_STM32_CAN1 is not set
+# CONFIG_STM32_CAN2 is not set
+# CONFIG_STM32_CCMDATARAM is not set
+# CONFIG_STM32_CRC is not set
+# CONFIG_STM32_CRYP is not set
+# CONFIG_STM32_DMA1 is not set
+# CONFIG_STM32_DMA2 is not set
+# CONFIG_STM32_DAC1 is not set
+# CONFIG_STM32_DAC2 is not set
+# CONFIG_STM32_DCMI is not set
+# CONFIG_STM32_ETHMAC is not set
+# CONFIG_STM32_FSMC is not set
+# CONFIG_STM32_HASH is not set
+# CONFIG_STM32_I2C1 is not set
+# CONFIG_STM32_I2C2 is not set
+# CONFIG_STM32_I2C3 is not set
+# CONFIG_STM32_IWDG is not set
+# CONFIG_STM32_OTGFS is not set
+# CONFIG_STM32_OTGHS is not set
+# CONFIG_STM32_PWR is not set
+# CONFIG_STM32_RNG is not set
+# CONFIG_STM32_SDIO is not set
+# CONFIG_STM32_SPI1 is not set
+# CONFIG_STM32_SPI2 is not set
+# CONFIG_STM32_SPI3 is not set
+CONFIG_STM32_SYSCFG=y
+# CONFIG_STM32_TIM1 is not set
+# CONFIG_STM32_TIM2 is not set
+# CONFIG_STM32_TIM3 is not set
+# CONFIG_STM32_TIM4 is not set
+# CONFIG_STM32_TIM5 is not set
+# CONFIG_STM32_TIM6 is not set
+# CONFIG_STM32_TIM7 is not set
+# CONFIG_STM32_TIM8 is not set
+# CONFIG_STM32_TIM9 is not set
+# CONFIG_STM32_TIM10 is not set
+# CONFIG_STM32_TIM11 is not set
+# CONFIG_STM32_TIM12 is not set
+# CONFIG_STM32_TIM13 is not set
+# CONFIG_STM32_TIM14 is not set
+# CONFIG_STM32_USART1 is not set
+CONFIG_STM32_USART2=y
+# CONFIG_STM32_USART3 is not set
+# CONFIG_STM32_UART4 is not set
+# CONFIG_STM32_UART5 is not set
+# CONFIG_STM32_USART6 is not set
+# CONFIG_STM32_WWDG is not set
+
+#
+# Alternate Pin Mapping
+#
+# CONFIG_STM32_JTAG_DISABLE is not set
+# CONFIG_STM32_JTAG_FULL_ENABLE is not set
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+CONFIG_STM32_JTAG_SW_ENABLE=y
+# CONFIG_STM32_FORCEPOWER is not set
+# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
+# CONFIG_STM32_CCMEXCLUDE is not set
+
+#
+# USB Host Configuration
+#
+
+#
+# Architecture Options
+#
+# CONFIG_ARCH_NOINTC is not set
+# CONFIG_ARCH_DMA is not set
+CONFIG_ARCH_STACKDUMP=y
+
+#
+# Board Settings
+#
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_SIZE=114688
+CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
+CONFIG_ARCH_INTERRUPTSTACK=0
+
+#
+# Boot options
+#
+# CONFIG_BOOT_RUNFROMEXTSRAM is not set
+CONFIG_BOOT_RUNFROMFLASH=y
+# CONFIG_BOOT_RUNFROMISRAM is not set
+# CONFIG_BOOT_RUNFROMSDRAM is not set
+# CONFIG_BOOT_COPYTORAM is not set
+
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_STM32F4_DISCOVERY=y
+# CONFIG_ARCH_BOARD_CUSTOM is not set
+CONFIG_ARCH_BOARD="stm32f4discovery"
+
+#
+# Common Board Options
+#
+CONFIG_ARCH_HAVE_LEDS=y
+CONFIG_ARCH_LEDS=y
+CONFIG_ARCH_HAVE_BUTTONS=y
+# CONFIG_ARCH_BUTTONS is not set
+CONFIG_ARCH_HAVE_IRQBUTTONS=y
+
+#
+# Board-Specific Options
+#
+
+#
+# RTOS Features
+#
+CONFIG_MSEC_PER_TICK=10
+CONFIG_RR_INTERVAL=200
+# CONFIG_SCHED_INSTRUMENTATION is not set
+CONFIG_TASK_NAME_SIZE=0
+# CONFIG_JULIAN_TIME is not set
+CONFIG_START_YEAR=2009
+CONFIG_START_MONTH=9
+CONFIG_START_DAY=21
+CONFIG_DEV_CONSOLE=y
+# CONFIG_MUTEX_TYPES is not set
+# CONFIG_PRIORITY_INHERITANCE is not set
+# CONFIG_FDCLONE_DISABLE is not set
+# CONFIG_FDCLONE_STDIO is not set
+CONFIG_SDCLONE_DISABLE=y
+# CONFIG_SCHED_WORKQUEUE is not set
+# CONFIG_SCHED_WAITPID is not set
+# CONFIG_SCHED_ATEXIT is not set
+# CONFIG_SCHED_ONEXIT is not set
+CONFIG_USER_ENTRYPOINT="ostest_main"
+CONFIG_DISABLE_OS_API=y
+# CONFIG_DISABLE_CLOCK is not set
+# CONFIG_DISABLE_POSIX_TIMERS is not set
+# CONFIG_DISABLE_PTHREAD is not set
+# CONFIG_DISABLE_SIGNALS is not set
+# CONFIG_DISABLE_MQUEUE is not set
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_DISABLE_ENVIRON=y
+CONFIG_DISABLE_POLL=y
+
+#
+# Sizes of configurable things (0 disables)
+#
+CONFIG_MAX_TASKS=16
+CONFIG_MAX_TASK_ARGS=4
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=8
+CONFIG_NFILE_STREAMS=8
+CONFIG_NAME_MAX=32
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=4
+CONFIG_PREALLOC_TIMERS=4
+
+#
+# Stack and heap information
+#
+# CONFIG_CUSTOM_STACK is not set
+CONFIG_IDLETHREAD_STACKSIZE=1024
+CONFIG_USERMAIN_STACKSIZE=2048
+CONFIG_PTHREAD_STACK_MIN=256
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+
+#
+# Device Drivers
+#
+CONFIG_DEV_NULL=y
+# CONFIG_DEV_ZERO is not set
+# CONFIG_LOOP is not set
+# CONFIG_RAMDISK is not set
+# CONFIG_CAN is not set
+# CONFIG_PWM is not set
+# CONFIG_I2C is not set
+CONFIG_ARCH_HAVE_I2CRESET=y
+# CONFIG_SPI is not set
+# CONFIG_RTC is not set
+# CONFIG_WATCHDOG is not set
+# CONFIG_ANALOG is not set
+# CONFIG_BCH is not set
+# CONFIG_INPUT is not set
+# CONFIG_LCD is not set
+# CONFIG_MMCSD is not set
+# CONFIG_MTD is not set
+# CONFIG_PIPES is not set
+# CONFIG_PM is not set
+# CONFIG_POWER is not set
+# CONFIG_SENSORS is not set
+# CONFIG_SERCOMM_CONSOLE is not set
+CONFIG_SERIAL=y
+CONFIG_DEV_LOWCONSOLE=y
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_USART2_SERIAL_CONSOLE=y
+# CONFIG_NO_SERIAL_CONSOLE is not set
+
+#
+# USART2 Configuration
+#
+CONFIG_USART2_RXBUFSIZE=128
+CONFIG_USART2_TXBUFSIZE=128
+CONFIG_USART2_BAUD=115200
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+# CONFIG_USBDEV is not set
+# CONFIG_USBHOST is not set
+# CONFIG_WIRELESS is not set
+
+#
+# System Logging Device Options
+#
+
+#
+# System Logging
+#
+# CONFIG_RAMLOG is not set
+
+#
+# Networking Support
+#
+# CONFIG_NET is not set
+
+#
+# File Systems
+#
+
+#
+# File system configuration
+#
+# CONFIG_FS_RAMMAP is not set
+
+#
+# System Logging
+#
+# CONFIG_SYSLOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_NX is not set
+
+#
+# Memory Management
+#
+# CONFIG_MM_SMALL is not set
+CONFIG_MM_REGIONS=2
+# CONFIG_GRAN is not set
+
+#
+# Binary Formats
+#
+# CONFIG_BINFMT_DISABLE is not set
+# CONFIG_NXFLAT is not set
+# CONFIG_ELF is not set
+CONFIG_SYMTAB_ORDEREDBYNAME=y
+
+#
+# Library Routines
+#
+CONFIG_STDIO_BUFFER_SIZE=64
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+# CONFIG_LIBM is not set
+# CONFIG_NOPRINTF_FIELDWIDTH is not set
+# CONFIG_LIBC_FLOATINGPOINT is not set
+# CONFIG_EOL_IS_CR is not set
+# CONFIG_EOL_IS_LF is not set
+# CONFIG_EOL_IS_BOTH_CRLF is not set
+CONFIG_EOL_IS_EITHER_CRLF=y
+# CONFIG_LIBC_STRERROR is not set
+# CONFIG_LIBC_PERROR_STDOUT is not set
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_LIB_SENDFILE_BUFSIZE=512
+# CONFIG_ARCH_ROMGETC is not set
+# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set
+
+#
+# Basic CXX Support
+#
+# CONFIG_HAVE_CXX is not set
+
+#
+# Application Configuration
+#
+
+#
+# Named Applications
+#
+# CONFIG_BUILTIN is not set
+
+#
+# Examples
+#
+# CONFIG_EXAMPLES_BUTTONS is not set
+# CONFIG_EXAMPLES_CAN is not set
+# CONFIG_EXAMPLES_CDCACM is not set
+# CONFIG_EXAMPLES_COMPOSITE is not set
+# CONFIG_EXAMPLES_DHCPD is not set
+# CONFIG_EXAMPLES_ELF is not set
+# CONFIG_EXAMPLES_FTPC is not set
+# CONFIG_EXAMPLES_FTPD is not set
+# CONFIG_EXAMPLES_HELLO is not set
+# CONFIG_EXAMPLES_HELLOXX is not set
+# CONFIG_EXAMPLES_JSON is not set
+# CONFIG_EXAMPLES_HIDKBD is not set
+# CONFIG_EXAMPLES_IGMP is not set
+# CONFIG_EXAMPLES_LCDRW is not set
+# CONFIG_EXAMPLES_MM is not set
+# CONFIG_EXAMPLES_MOUNT is not set
+# CONFIG_EXAMPLES_MODBUS is not set
+# CONFIG_EXAMPLES_NETTEST is not set
+# CONFIG_EXAMPLES_NSH is not set
+# CONFIG_EXAMPLES_NULL is not set
+# CONFIG_EXAMPLES_NX is not set
+# CONFIG_EXAMPLES_NXCONSOLE is not set
+# CONFIG_EXAMPLES_NXFFS is not set
+# CONFIG_EXAMPLES_NXFLAT is not set
+# CONFIG_EXAMPLES_NXHELLO is not set
+# CONFIG_EXAMPLES_NXIMAGE is not set
+# CONFIG_EXAMPLES_NXLINES is not set
+# CONFIG_EXAMPLES_NXTEXT is not set
+CONFIG_EXAMPLES_OSTEST=y
+# CONFIG_EXAMPLES_OSTEST_BUILTIN is not set
+CONFIG_EXAMPLES_OSTEST_LOOPS=1
+CONFIG_EXAMPLES_OSTEST_STACKSIZE=2048
+CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS=3
+CONFIG_EXAMPLES_OSTEST_RR_RANGE=10000
+CONFIG_EXAMPLES_OSTEST_RR_RUNS=10
+# CONFIG_EXAMPLES_PASHELLO is not set
+# CONFIG_EXAMPLES_PIPE is not set
+# CONFIG_EXAMPLES_POLL is not set
+# CONFIG_EXAMPLES_QENCODER is not set
+# CONFIG_EXAMPLES_RGMP is not set
+# CONFIG_EXAMPLES_ROMFS is not set
+# CONFIG_EXAMPLES_SENDMAIL is not set
+# CONFIG_EXAMPLES_SERLOOP is not set
+# CONFIG_EXAMPLES_TELNETD is not set
+# CONFIG_EXAMPLES_THTTPD is not set
+# CONFIG_EXAMPLES_TIFF is not set
+# CONFIG_EXAMPLES_TOUCHSCREEN is not set
+# CONFIG_EXAMPLES_UDP is not set
+# CONFIG_EXAMPLES_UIP is not set
+# CONFIG_EXAMPLES_USBSERIAL is not set
+# CONFIG_EXAMPLES_USBMSC is not set
+# CONFIG_EXAMPLES_USBTERM is not set
+# CONFIG_EXAMPLES_WATCHDOG is not set
+# CONFIG_EXAMPLES_WLAN is not set
+
+#
+# Interpreters
+#
+
+#
+# Interpreters
+#
+# CONFIG_INTERPRETERS_FICL is not set
+# CONFIG_INTERPRETERS_PCODE is not set
+
+#
+# Network Utilities
+#
+
+#
+# Networking Utilities
+#
+# CONFIG_NETUTILS_CODECS is not set
+# CONFIG_NETUTILS_DHCPC is not set
+# CONFIG_NETUTILS_DHCPD is not set
+# CONFIG_NETUTILS_FTPC is not set
+# CONFIG_NETUTILS_FTPD is not set
+# CONFIG_NETUTILS_JSON is not set
+# CONFIG_NETUTILS_RESOLV is not set
+# CONFIG_NETUTILS_SMTP is not set
+# CONFIG_NETUTILS_TELNETD is not set
+# CONFIG_NETUTILS_TFTPC is not set
+# CONFIG_NETUTILS_THTTPD is not set
+# CONFIG_NETUTILS_UIPLIB is not set
+# CONFIG_NETUTILS_WEBCLIENT is not set
+
+#
+# ModBus
+#
+
+#
+# FreeModbus
+#
+# CONFIG_MODBUS is not set
+
+#
+# NSH Library
+#
+# CONFIG_NSH_LIBRARY is not set
+
+#
+# NxWidgets/NxWM
+#
+
+#
+# System NSH Add-Ons
+#
+
+#
+# Custom Free Memory Command
+#
+# CONFIG_SYSTEM_FREE is not set
+
+#
+# I2C tool
+#
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# readline()
+#
+# CONFIG_SYSTEM_READLINE is not set
+
+#
+# Power Off
+#
+# CONFIG_SYSTEM_POWEROFF is not set
+
+#
+# RAMTRON
+#
+# CONFIG_SYSTEM_RAMTRON is not set
+
+#
+# SD Card
+#
+# CONFIG_SYSTEM_SDCARD is not set
+
+#
+# Sysinfo
+#
+# CONFIG_SYSTEM_SYSINFO is not set
diff --git a/nuttx-configs/px4fmu-v1/ostest/setenv.sh b/nuttx-configs/px4fmu-v1/ostest/setenv.sh
new file mode 100755
index 000000000..a67fdc5a8
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/ostest/setenv.sh
@@ -0,0 +1,75 @@
+#!/bin/bash
+# configs/stm32f4discovery/ostest/setenv.sh
+#
+#   Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$_" = "$0" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+  echo "This script must be executed from the top-level NuttX build directory"
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+  export PATH_ORIG="${PATH}"
+fi
+
+# This is the Cygwin path to the location where I installed the RIDE
+# toolchain under windows.  You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This is the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows.  You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# These are the Cygwin paths to the locations where I installed the Atollic
+# toolchain under windows.  You will also have to edit this if you install
+# the Atollic toolchain in any other location.  /usr/bin is added before
+# the Atollic bin path because there is are binaries named gcc.exe and g++.exe
+# at those locations as well.
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin"
+#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin"
+
+# This is the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld b/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld
new file mode 100644
index 000000000..1f29f02f5
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld
@@ -0,0 +1,129 @@
+/****************************************************************************
+ * configs/stm32f4discovery/scripts/gnu-elf.ld
+ *
+ *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+SECTIONS
+{
+  .text 0x00000000 :
+    {
+      _stext = . ;
+      *(.text)
+      *(.text.*)
+      *(.gnu.warning)
+      *(.stub)
+      *(.glue_7)
+      *(.glue_7t)
+      *(.jcr)
+
+      /* C++ support:  The .init and .fini sections contain specific logic
+       * to manage static constructors and destructors.
+       */
+
+      *(.gnu.linkonce.t.*)
+      *(.init)             /* Old ABI */
+      *(.fini)             /* Old ABI */
+      _etext = . ;
+    }
+
+  .rodata :
+    {
+      _srodata = . ;
+      *(.rodata)
+      *(.rodata1)
+      *(.rodata.*)
+      *(.gnu.linkonce.r*)
+      _erodata = . ;
+    }
+
+  .data :
+    {
+      _sdata = . ;
+      *(.data)
+      *(.data1)
+      *(.data.*)
+      *(.gnu.linkonce.d*)
+      _edata = . ;
+    }
+
+  /* C++ support. For each global and static local C++ object,
+   * GCC creates a small subroutine to construct the object. Pointers
+   * to these routines (not the routines themselves) are stored as
+   * simple, linear arrays in the .ctors section of the object file.
+   * Similarly, pointers to global/static destructor routines are
+   * stored in .dtors.
+   */
+
+  .ctors :
+    {
+      _sctors = . ;
+      *(.ctors)       /* Old ABI:  Unallocated */
+      *(.init_array)  /* New ABI:  Allocated */
+      _edtors = . ;
+    }
+
+  .dtors :
+    {
+      _sdtors = . ;
+      *(.dtors)       /* Old ABI:  Unallocated */
+      *(.fini_array)  /* New ABI:  Allocated */
+      _edtors = . ;
+    }
+
+  .bss :
+    {
+      _sbss = . ;
+      *(.bss)
+      *(.bss.*)
+      *(.sbss)
+      *(.sbss.*)
+      *(.gnu.linkonce.b*)
+      *(COMMON)
+      _ebss = . ;
+    }
+
+    /* Stabs debugging sections.    */
+
+    .stab 0 : { *(.stab) }
+    .stabstr 0 : { *(.stabstr) }
+    .stab.excl 0 : { *(.stab.excl) }
+    .stab.exclstr 0 : { *(.stab.exclstr) }
+    .stab.index 0 : { *(.stab.index) }
+    .stab.indexstr 0 : { *(.stab.indexstr) }
+    .comment 0 : { *(.comment) }
+    .debug_abbrev 0 : { *(.debug_abbrev) }
+    .debug_info 0 : { *(.debug_info) }
+    .debug_line 0 : { *(.debug_line) }
+    .debug_pubnames 0 : { *(.debug_pubnames) }
+    .debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/px4fmu-v1/scripts/ld.script b/nuttx-configs/px4fmu-v1/scripts/ld.script
new file mode 100644
index 000000000..f6560743b
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/scripts/ld.script
@@ -0,0 +1,123 @@
+/****************************************************************************
+ * configs/stm32f4discovery/scripts/ld.script
+ *
+ *   Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and
+ * 192Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of CCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address
+ * range.
+ */
+
+MEMORY
+{
+    flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K
+    sram (rwx) : ORIGIN = 0x20000000, LENGTH = 112K
+}
+
+OUTPUT_ARCH(arm)
+EXTERN(_vectors)
+ENTRY(_stext)
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	.init_section : {
+		_sinit = ABSOLUTE(.);
+		*(.init_array .init_array.*)
+		_einit = ABSOLUTE(.);
+	} > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/px4fmu-v1/src/Makefile b/nuttx-configs/px4fmu-v1/src/Makefile
new file mode 100644
index 000000000..6ef8b7d6a
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/px4fmu-v1/src/Makefile
+#
+#   Copyright (C) 2013 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		= 
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	$(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	$(Q) touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
+
diff --git a/nuttx-configs/px4fmu-v1/src/empty.c b/nuttx-configs/px4fmu-v1/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so 
+ * we have this empty source file to keep it company.
+ */
diff --git a/nuttx-configs/px4fmu-v1/tools/px_mkfw.py b/nuttx-configs/px4fmu-v1/tools/px_mkfw.py
new file mode 100755
index 000000000..9f4ddad62
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/tools/px_mkfw.py
@@ -0,0 +1,110 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# PX4 firmware image generator
+#
+# The PX4 firmware file is a JSON-encoded Python object, containing
+# metadata fields and a zlib-compressed base64-encoded firmware image.
+#
+
+import sys
+import argparse
+import json
+import base64
+import zlib
+import time
+import subprocess
+
+#
+# Construct a basic firmware description
+#
+def mkdesc():
+	proto = {}
+	proto['magic']		= "PX4FWv1"
+	proto['board_id']	= 0
+	proto['board_revision']	= 0
+	proto['version']	= ""
+	proto['summary']	= ""
+	proto['description']	= ""
+	proto['git_identity']	= ""
+	proto['build_time']	= 0
+	proto['image']		= base64.b64encode(bytearray())
+	proto['image_size']	= 0
+	return proto
+
+# Parse commandline
+parser = argparse.ArgumentParser(description="Firmware generator for the PX autopilot system.")
+parser.add_argument("--prototype",	action="store", help="read a prototype description from a file")
+parser.add_argument("--board_id",	action="store", help="set the board ID required")
+parser.add_argument("--board_revision",	action="store", help="set the board revision required")
+parser.add_argument("--version",	action="store", help="set a version string")
+parser.add_argument("--summary",	action="store", help="set a brief description")
+parser.add_argument("--description",	action="store", help="set a longer description")
+parser.add_argument("--git_identity",	action="store", help="the working directory to check for git identity")
+parser.add_argument("--image",		action="store", help="the firmware image")
+args = parser.parse_args()
+
+# Fetch the firmware descriptor prototype if specified
+if args.prototype != None:
+	f = open(args.prototype,"r")
+	desc = json.load(f)
+	f.close()
+else:
+	desc = mkdesc()
+
+desc['build_time'] 		= int(time.time())
+
+if args.board_id != None:
+	desc['board_id']	= int(args.board_id)
+if args.board_revision != None:
+	desc['board_revision']	= int(args.board_revision)
+if args.version != None:
+	desc['version']		= str(args.version)
+if args.summary != None:
+	desc['summary']		= str(args.summary)
+if args.description != None:
+	desc['description']	= str(args.description)
+if args.git_identity != None:
+	cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"])
+	p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout
+	desc['git_identity']	= p.read().strip()
+	p.close()
+if args.image != None:
+	f = open(args.image, "rb")
+	bytes = f.read()
+	desc['image_size'] = len(bytes)
+	desc['image'] = base64.b64encode(zlib.compress(bytes,9))
+
+print json.dumps(desc, indent=4)
diff --git a/nuttx-configs/px4fmu-v1/tools/px_uploader.py b/nuttx-configs/px4fmu-v1/tools/px_uploader.py
new file mode 100755
index 000000000..3b23f4f83
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/tools/px_uploader.py
@@ -0,0 +1,416 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Serial firmware uploader for the PX4FMU bootloader
+#
+# The PX4 firmware file is a JSON-encoded Python object, containing
+# metadata fields and a zlib-compressed base64-encoded firmware image.
+#
+# The uploader uses the following fields from the firmware file:
+#
+# image
+#	The firmware that will be uploaded.
+# image_size
+#	The size of the firmware in bytes.
+# board_id
+#	The board for which the firmware is intended.
+# board_revision
+#	Currently only used for informational purposes.
+#
+
+import sys
+import argparse
+import binascii
+import serial
+import os
+import struct
+import json
+import zlib
+import base64
+import time
+import array
+
+from sys import platform as _platform
+
+class firmware(object):
+	'''Loads a firmware file'''
+
+	desc = {}
+	image = bytes()
+	crctab = array.array('I', [
+		0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+		0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+		0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+		0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+		0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+		0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+		0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+		0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+		0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+		0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+		0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+		0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+		0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+		0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+		0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+		0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+		0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+		0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+		0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+		0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+		0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+		0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+		0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+		0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+		0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+		0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+		0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+		0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+		0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+		0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+		0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+		0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ])
+	crcpad = bytearray('\xff\xff\xff\xff')
+
+	def __init__(self, path):
+
+		# read the file
+		f = open(path, "r")
+		self.desc = json.load(f)
+		f.close()
+
+		self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))
+
+		# pad image to 4-byte length
+		while ((len(self.image) % 4) != 0):
+			self.image.append('\xff')
+
+	def property(self, propname):
+		return self.desc[propname]
+
+	def __crc32(self, bytes, state):
+		for byte in bytes:
+			index = (state ^ byte) & 0xff
+			state = self.crctab[index] ^ (state >> 8)
+		return state
+
+	def crc(self, padlen):
+		state = self.__crc32(self.image, int(0))
+		for i in range(len(self.image), (padlen - 1), 4):
+			state = self.__crc32(self.crcpad, state)
+		return state
+
+class uploader(object):
+	'''Uploads a firmware file to the PX FMU bootloader'''
+
+	# protocol bytes
+	INSYNC		= chr(0x12)
+	EOC		= chr(0x20)
+
+	# reply bytes
+	OK		= chr(0x10)
+	FAILED		= chr(0x11)
+	INVALID		= chr(0x13)	# rev3+
+
+	# command bytes
+	NOP		= chr(0x00)	# guaranteed to be discarded by the bootloader
+	GET_SYNC	= chr(0x21)
+	GET_DEVICE	= chr(0x22)
+	CHIP_ERASE	= chr(0x23)
+	CHIP_VERIFY	= chr(0x24)	# rev2 only
+	PROG_MULTI	= chr(0x27)
+	READ_MULTI	= chr(0x28)	# rev2 only
+	GET_CRC		= chr(0x29)	# rev3+
+	REBOOT		= chr(0x30)
+	
+	INFO_BL_REV	= chr(1)	# bootloader protocol revision
+	BL_REV_MIN	= 2		# minimum supported bootloader protocol 
+	BL_REV_MAX	= 3		# maximum supported bootloader protocol 
+	INFO_BOARD_ID	= chr(2)	# board type
+	INFO_BOARD_REV	= chr(3)	# board revision
+	INFO_FLASH_SIZE	= chr(4)	# max firmware size in bytes
+
+	PROG_MULTI_MAX	= 60		# protocol max is 255, must be multiple of 4
+	READ_MULTI_MAX	= 60		# protocol max is 255, something overflows with >= 64
+
+	def __init__(self, portname, baudrate):
+		# open the port, keep the default timeout short so we can poll quickly
+		self.port = serial.Serial(portname, baudrate, timeout=0.25)
+
+	def close(self):
+		if self.port is not None:
+			self.port.close()
+
+	def __send(self, c):
+#		print("send " + binascii.hexlify(c))
+		self.port.write(str(c))
+
+	def __recv(self, count = 1):
+		c = self.port.read(count)
+		if len(c) < 1:
+			raise RuntimeError("timeout waiting for data")
+#		print("recv " + binascii.hexlify(c))
+		return c
+
+	def __recv_int(self):
+		raw = self.__recv(4)
+		val = struct.unpack("<I", raw)
+		return val[0]
+
+	def __getSync(self):
+		self.port.flush()
+		c = self.__recv()
+		if c is not self.INSYNC:
+			raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c))
+		c = self.__recv()
+		if c == self.INVALID:
+			raise RuntimeError("bootloader reports INVALID OPERATION")
+		if c == self.FAILED:
+			raise RuntimeError("bootloader reports OPERATION FAILED")
+		if c != self.OK:
+			raise RuntimeError("unexpected response 0x%x instead of OK" % ord(c))
+
+	# attempt to get back into sync with the bootloader
+	def __sync(self):
+		# send a stream of ignored bytes longer than the longest possible conversation
+		# that we might still have in progress
+#		self.__send(uploader.NOP * (uploader.PROG_MULTI_MAX + 2))
+		self.port.flushInput()
+		self.__send(uploader.GET_SYNC 
+				+ uploader.EOC)
+		self.__getSync()
+		
+#	def __trySync(self):
+#		c = self.__recv()
+#		if (c != self.INSYNC):
+#			#print("unexpected 0x%x instead of INSYNC" % ord(c))
+#			return False;
+#		c = self.__recv()
+#		if (c != self.OK):
+#			#print("unexpected 0x%x instead of OK" % ord(c))
+#			return False
+#		return True
+
+	# send the GET_DEVICE command and wait for an info parameter
+	def __getInfo(self, param):
+		self.__send(uploader.GET_DEVICE + param + uploader.EOC)
+		value = self.__recv_int()
+		self.__getSync()
+		return value
+
+	# send the CHIP_ERASE command and wait for the bootloader to become ready
+	def __erase(self):
+		self.__send(uploader.CHIP_ERASE 
+				+ uploader.EOC)
+		# erase is very slow, give it 10s
+		deadline = time.time() + 10
+		while time.time() < deadline:
+			try:
+				self.__getSync()
+				return
+			except RuntimeError as ex:
+				# we timed out, that's OK
+				continue
+
+		raise RuntimeError("timed out waiting for erase")
+
+	# send a PROG_MULTI command to write a collection of bytes
+	def __program_multi(self, data):
+		self.__send(uploader.PROG_MULTI
+				+ chr(len(data)))
+		self.__send(data)
+		self.__send(uploader.EOC)
+		self.__getSync()
+		
+	# verify multiple bytes in flash
+	def __verify_multi(self, data):
+		self.__send(uploader.READ_MULTI
+				+ chr(len(data))
+				+ uploader.EOC)
+		self.port.flush()
+		programmed = self.__recv(len(data))
+		if programmed != data:
+			print("got    " + binascii.hexlify(programmed))
+			print("expect " + binascii.hexlify(data))
+			return False
+		self.__getSync()
+		return True
+		
+	# send the reboot command
+	def __reboot(self):
+		self.__send(uploader.REBOOT
+				+ uploader.EOC)
+		self.port.flush()
+
+		# v3+ can report failure if the first word flash fails
+		if self.bl_rev >= 3:
+			self.__getSync()
+
+	# split a sequence into a list of size-constrained pieces
+	def __split_len(self, seq, length):
+    		return [seq[i:i+length] for i in range(0, len(seq), length)]
+
+	# upload code
+	def __program(self, fw):
+		code = fw.image
+		groups = self.__split_len(code, uploader.PROG_MULTI_MAX)
+		for bytes in groups:
+			self.__program_multi(bytes)
+
+	# verify code
+	def __verify_v2(self, fw):
+		self.__send(uploader.CHIP_VERIFY
+				+ uploader.EOC)
+		self.__getSync()
+		code = fw.image
+		groups = self.__split_len(code, uploader.READ_MULTI_MAX)
+		for bytes in groups:
+			if (not self.__verify_multi(bytes)):
+				raise RuntimeError("Verification failed")
+
+	def __verify_v3(self, fw):
+		expect_crc = fw.crc(self.fw_maxsize)
+		self.__send(uploader.GET_CRC
+				+ uploader.EOC)
+		report_crc = self.__recv_int()
+		self.__getSync()
+		if report_crc != expect_crc:
+			print("Expected 0x%x" % expect_crc)
+			print("Got      0x%x" % report_crc)
+			raise RuntimeError("Program CRC failed")
+
+	# get basic data about the board
+	def identify(self):
+		# make sure we are in sync before starting
+		self.__sync()
+
+		# get the bootloader protocol ID first
+		self.bl_rev = self.__getInfo(uploader.INFO_BL_REV)
+		if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX):
+			print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)
+			raise RuntimeError("Bootloader protocol mismatch")
+
+		self.board_type = self.__getInfo(uploader.INFO_BOARD_ID)
+		self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV)
+		self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)
+
+	# upload the firmware
+	def upload(self, fw):
+		# Make sure we are doing the right thing
+		if self.board_type != fw.property('board_id'):
+			raise RuntimeError("Firmware not suitable for this board (run 'make configure_px4fmu && make clean' or 'make configure_px4io && make clean' to reconfigure).")
+		if self.fw_maxsize < fw.property('image_size'):
+			raise RuntimeError("Firmware image is too large for this board")
+
+		print("erase...")
+		self.__erase()
+
+		print("program...")
+		self.__program(fw)
+
+		print("verify...")
+		if self.bl_rev == 2:
+			self.__verify_v2(fw)
+		else:
+			self.__verify_v3(fw)
+
+		print("done, rebooting.")
+		self.__reboot()
+		self.port.close()
+	
+
+# Parse commandline arguments
+parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.")
+parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached")
+parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.")
+parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
+args = parser.parse_args()
+
+# Load the firmware file
+fw = firmware(args.firmware)
+print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
+
+# Spin waiting for a device to show up
+while True:
+	for port in args.port.split(","):
+
+		#print("Trying %s" % port)
+
+		# create an uploader attached to the port
+		try:
+			if "linux" in _platform:
+			# Linux, don't open Mac OS and Win ports
+				if not "COM" in port and not "tty.usb" in port:
+					up = uploader(port, args.baud)
+			elif "darwin" in _platform:
+				# OS X, don't open Windows and Linux ports
+				if not "COM" in port and not "ACM" in port:
+					up = uploader(port, args.baud)
+			elif "win" in _platform:
+				# Windows, don't open POSIX ports
+				if not "/" in port:
+					up = uploader(port, args.baud)
+		except:
+			# open failed, rate-limit our attempts
+			time.sleep(0.05)
+
+			# and loop to the next port
+			continue
+
+		# port is open, try talking to it
+		try:
+			# identify the bootloader
+			up.identify()
+			print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
+
+		except:
+			# most probably a timeout talking to the port, no bootloader
+			continue
+
+		try:
+			# ok, we have a bootloader, try flashing it
+			up.upload(fw)
+
+		except RuntimeError as ex:
+
+			# print the error
+			print("ERROR: %s" % ex.args)
+
+		finally:
+			# always close the port
+			up.close()
+
+		# we could loop here if we wanted to wait for more boards...
+		sys.exit(0)
diff --git a/nuttx-configs/px4fmu-v1/tools/upload.sh b/nuttx-configs/px4fmu-v1/tools/upload.sh
new file mode 100755
index 000000000..4e6597b3a
--- /dev/null
+++ b/nuttx-configs/px4fmu-v1/tools/upload.sh
@@ -0,0 +1,27 @@
+#!/bin/sh
+#
+# Wrapper to upload a PX4 firmware binary
+#
+TOOLS=`dirname $0`
+MKFW=${TOOLS}/px_mkfw.py
+UPLOAD=${TOOLS}/px_uploader.py
+
+BINARY=nuttx.bin
+PAYLOAD=nuttx.px4
+PORTS="/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4"
+
+function abort() {
+	echo "ABORT: $*"
+	exit 1
+}
+
+if [ ! -f ${MKFW} -o ! -f ${UPLOAD} ]; then
+	abort "Missing tools ${MKFW} and/or ${UPLOAD}"
+fi
+if [ ! -f ${BINARY} ]; then
+	abort "Missing nuttx binary in current directory."
+fi
+
+rm -f ${PAYLOAD}
+${MKFW} --board_id 5 --image ${BINARY} > ${PAYLOAD}
+${UPLOAD} --port ${PORTS} ${PAYLOAD}
diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h
index a6cdfb4d2..507df70a2 100755
--- a/nuttx-configs/px4fmu-v2/include/board.h
+++ b/nuttx-configs/px4fmu-v2/include/board.h
@@ -194,11 +194,6 @@
 
 #define DMAMAP_SDIO DMAMAP_SDIO_1
 
-/* High-resolution timer
- */
-#define HRT_TIMER		8	/* use timer8 for the HRT */
-#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
-
 /* Alternate function pin selections ************************************************/
 
 /*
@@ -232,35 +227,12 @@
 #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
 
-/*
- * PWM
- *
- * Six PWM outputs are configured.
- *
- * Pins:
- *
- * CH1 : PE14 : TIM1_CH4
- * CH2 : PE13 : TIM1_CH3
- * CH3 : PE11 : TIM1_CH2
- * CH4 : PE9  : TIM1_CH1
- * CH5 : PD13 : TIM4_CH2
- * CH6 : PD14 : TIM4_CH3
- *
- */
-#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_2
-#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_2
-#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_2
-#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_2
-#define GPIO_TIM4_CH2OUT	GPIO_TIM4_CH2OUT_2
-#define GPIO_TIM4_CH3OUT	GPIO_TIM4_CH3OUT_2
-
 /* 
  * CAN
  *
  * CAN1 is routed to the onboard transceiver. 
  * CAN2 is routed to the expansion connector.
  */
-
 #define GPIO_CAN1_RX	GPIO_CAN1_RX_3
 #define GPIO_CAN1_TX	GPIO_CAN1_TX_3
 #define GPIO_CAN2_RX	GPIO_CAN2_RX_1
@@ -283,20 +255,6 @@
 #define GPIO_I2C2_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
 #define GPIO_I2C2_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
 
-/*
- * I2C busses
- */
-#define PX4_I2C_BUS_EXPANSION	1
-#define PX4_I2C_BUS_LED		2
-
-/*
- * Devices on the onboard bus.
- *
- * Note that these are unshifted addresses.
- */
-#define PX4_I2C_OBDEV_LED	0x55
-#define PX4_I2C_OBDEV_HMC5883	0x1e
-
 /*
  * SPI
  *
@@ -310,22 +268,6 @@
 #define GPIO_SPI2_MOSI	GPIO_SPI2_MOSI_1
 #define GPIO_SPI2_SCK	GPIO_SPI2_SCK_2
 
-/*
- * Use these in place of the spi_dev_e enumeration to
- * select a specific SPI device on SPI1
- */
-#define PX4_SPIDEV_GYRO		1
-#define PX4_SPIDEV_ACCEL_MAG	2
-#define PX4_SPIDEV_BARO		3
-
-/*
- * Tone alarm output
- */
-#define TONE_ALARM_TIMER	2	/* timer 2 */
-#define TONE_ALARM_CHANNEL	1	/* channel 1 */
-#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
-#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
-
 /************************************************************************************
  * Public Data
  ************************************************************************************/
diff --git a/nuttx-configs/px4io-v1/Kconfig b/nuttx-configs/px4io-v1/Kconfig
new file mode 100644
index 000000000..fbf74d7f0
--- /dev/null
+++ b/nuttx-configs/px4io-v1/Kconfig
@@ -0,0 +1,21 @@
+#
+# For a description of the syntax of this configuration file,
+# see misc/tools/kconfig-language.txt.
+#
+
+if ARCH_BOARD_PX4IO_V1
+
+config HRT_TIMER
+	bool "High resolution timer support"
+	default y
+	---help---
+		Enable high resolution timer for PPM capture and system clocks.
+
+config HRT_PPM
+	bool "PPM input capture"
+	default y
+	depends on HRT_TIMER
+	---help---
+		Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs)
+
+endif
diff --git a/nuttx-configs/px4io-v1/README.txt b/nuttx-configs/px4io-v1/README.txt
new file mode 100755
index 000000000..9b1615f42
--- /dev/null
+++ b/nuttx-configs/px4io-v1/README.txt
@@ -0,0 +1,806 @@
+README
+======
+
+This README discusses issues unique to NuttX configurations for the
+STMicro STM3210E-EVAL development board.
+
+Contents
+========
+
+  - Development Environment
+  - GNU Toolchain Options
+  - IDEs
+  - NuttX buildroot Toolchain
+  - DFU and JTAG
+  - OpenOCD
+  - LEDs
+  - Temperature Sensor
+  - RTC
+  - STM3210E-EVAL-specific Configuration Options
+  - Configurations
+
+Development Environment
+=======================
+
+  Either Linux or Cygwin on Windows can be used for the development environment.
+  The source has been built only using the GNU toolchain (see below).  Other
+  toolchains will likely cause problems. Testing was performed using the Cygwin
+  environment because the Raisonance R-Link emulatator and some RIDE7 development tools
+  were used and those tools works only under Windows.
+
+GNU Toolchain Options
+=====================
+
+  The NuttX make system has been modified to support the following different
+  toolchain options.
+
+  1. The CodeSourcery GNU toolchain,
+  2. The devkitARM GNU toolchain,
+  3. Raisonance GNU toolchain, or
+  4. The NuttX buildroot Toolchain (see below).
+
+  All testing has been conducted using the NuttX buildroot toolchain.  However,
+  the make system is setup to default to use the devkitARM toolchain.  To use
+  the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to
+  add one of the following configuration options to your .config (or defconfig)
+  file:
+
+    CONFIG_STM32_CODESOURCERYW=y  : CodeSourcery under Windows
+    CONFIG_STM32_CODESOURCERYL=y  : CodeSourcery under Linux
+    CONFIG_STM32_DEVKITARM=y      : devkitARM under Windows
+    CONFIG_STM32_RAISONANCE=y     : Raisonance RIDE7 under Windows
+    CONFIG_STM32_BUILDROOT=y      : NuttX buildroot under Linux or Cygwin (default)
+
+  If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify
+  the PATH in the setenv.h file if your make cannot find the tools.
+
+  NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are
+  Windows native toolchains.  The CodeSourcey (for Linux) and NuttX buildroot
+  toolchains are Cygwin and/or Linux native toolchains. There are several limitations
+  to using a Windows based toolchain in a Cygwin environment.  The three biggest are:
+
+  1. The Windows toolchain cannot follow Cygwin paths.  Path conversions are
+     performed automatically in the Cygwin makefiles using the 'cygpath' utility
+     but you might easily find some new path problems.  If so, check out 'cygpath -w'
+
+  2. Windows toolchains cannot follow Cygwin symbolic links.  Many symbolic links
+     are used in Nuttx (e.g., include/arch).  The make system works around these
+     problems for the Windows tools by copying directories instead of linking them.
+     But this can also cause some confusion for you:  For example, you may edit
+     a file in a "linked" directory and find that your changes had no effect.
+     That is because you are building the copy of the file in the "fake" symbolic
+     directory.  If you use a Windows toolchain, you should get in the habit of
+     making like this:
+
+       make clean_context all
+
+     An alias in your .bashrc file might make that less painful.
+
+  3. Dependencies are not made when using Windows versions of the GCC.  This is
+     because the dependencies are generated using Windows pathes which do not
+     work with the Cygwin make.
+
+     Support has been added for making dependencies with the windows-native toolchains.
+     That support can be enabled by modifying your Make.defs file as follows:
+
+    -  MKDEP                = $(TOPDIR)/tools/mknulldeps.sh
+    +  MKDEP                = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)"
+
+     If you have problems with the dependency build (for example, if you are not
+     building on C:), then you may need to modify tools/mkdeps.sh
+
+  NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization
+  level of -Os (See Make.defs).  It will work with -O0, -O1, or -O2, but not with
+  -Os.
+
+  NOTE 2: The devkitARM toolchain includes a version of MSYS make.  Make sure that
+  the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM
+  path or will get the wrong version of make.
+
+IDEs
+====
+
+  NuttX is built using command-line make.  It can be used with an IDE, but some
+  effort will be required to create the project (There is a simple RIDE project
+  in the RIDE subdirectory).
+  
+  Makefile Build
+  --------------
+  Under Eclipse, it is pretty easy to set up an "empty makefile project" and
+  simply use the NuttX makefile to build the system.  That is almost for free
+  under Linux.  Under Windows, you will need to set up the "Cygwin GCC" empty
+  makefile project in order to work with Windows (Google for "Eclipse Cygwin" -
+  there is a lot of help on the internet).
+
+  Native Build
+  ------------
+  Here are a few tips before you start that effort:
+
+  1) Select the toolchain that you will be using in your .config file
+  2) Start the NuttX build at least one time from the Cygwin command line
+     before trying to create your project.  This is necessary to create
+     certain auto-generated files and directories that will be needed.
+  3) Set up include pathes:  You will need include/, arch/arm/src/stm32,
+     arch/arm/src/common, arch/arm/src/armv7-m, and sched/.
+  4) All assembly files need to have the definition option -D __ASSEMBLY__
+     on the command line.
+
+  Startup files will probably cause you some headaches.  The NuttX startup file
+  is arch/arm/src/stm32/stm32_vectors.S.  With RIDE, I have to build NuttX
+  one time from the Cygwin command line in order to obtain the pre-built
+  startup object needed by RIDE.
+
+NuttX buildroot Toolchain
+=========================
+
+  A GNU GCC-based toolchain is assumed.  The files */setenv.sh should
+  be modified to point to the correct path to the Cortex-M3 GCC toolchain (if
+  different from the default in your PATH variable).
+
+  If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX
+  SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573).
+  This GNU toolchain builds and executes in the Linux or Cygwin environment.
+
+  1. You must have already configured Nuttx in <some-dir>/nuttx.
+
+     cd tools
+     ./configure.sh stm3210e-eval/<sub-dir>
+
+  2. Download the latest buildroot package into <some-dir>
+
+  3. unpack the buildroot tarball.  The resulting directory may
+     have versioning information on it like buildroot-x.y.z.  If so,
+     rename <some-dir>/buildroot-x.y.z to <some-dir>/buildroot.
+
+  4. cd <some-dir>/buildroot
+
+  5. cp configs/cortexm3-defconfig-4.3.3 .config
+
+  6. make oldconfig
+
+  7. make
+
+  8. Edit setenv.h, if necessary, so that the PATH variable includes
+     the path to the newly built binaries.
+
+  See the file configs/README.txt in the buildroot source tree.  That has more
+  detailed PLUS some special instructions that you will need to follow if you are
+  building a Cortex-M3 toolchain for Cygwin under Windows.
+
+DFU and JTAG
+============
+
+  Enbling Support for the DFU Bootloader
+  --------------------------------------
+  The linker files in these projects can be configured to indicate that you
+  will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU)
+  loader or via some JTAG emulator.  You can specify the DFU bootloader by
+  adding the following line:
+
+    CONFIG_STM32_DFU=y
+
+  to your .config file. Most of the configurations in this directory are set
+  up to use the DFU loader.
+
+  If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning
+  of FLASH (0x08000000) but will be offset to 0x08003000.  This offset is needed
+  to make space for the DFU loader and 0x08003000 is where the DFU loader expects
+  to find new applications at boot time.  If you need to change that origin for some
+  other bootloader, you will need to edit the file(s) ld.script.dfu for each
+  configuration.
+
+  The DFU SE PC-based software is available from the STMicro website,
+  http://www.st.com.  General usage instructions:
+  
+  1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU
+     file (nuttx.dfu)... see below for details.
+  2. Connect the STM3210E-EVAL board to your computer using a USB
+     cable.
+  3. Start the DFU loader on the STM3210E-EVAL board.  You do this by
+     resetting the board while holding the "Key" button.  Windows should
+     recognize that the DFU loader has been installed.
+  3. Run the DFU SE program to load nuttx.dfu into FLASH.
+
+  What if the DFU loader is not in FLASH?  The loader code is available
+  inside of the Demo dirctory of the USBLib ZIP file that can be downloaded
+  from the STMicro Website.  You can build it using RIDE (or other toolchains);
+  you will need a JTAG emulator to burn it into FLASH the first time.
+
+  In order to use STMicro's built-in DFU loader, you will have to get
+  the NuttX binary into a special format with a .dfu extension.  The
+  DFU SE PC_based software installation includes a file "DFU File Manager"
+  conversion program that a file in Intel Hex format to the special DFU
+  format.  When you successfully build NuttX, you will find a file called
+  nutt.ihx in the top-level directory.  That is the file that you should
+  provide to the DFU File Manager.  You will need to rename it to nuttx.hex
+  in order to find it with the DFU File Manager. You will end up with
+  a file called nuttx.dfu that you can use with the STMicro DFU SE program.
+
+  Enabling JTAG
+  -------------
+  If you are not using the DFU, then you will probably also need to enable
+  JTAG support.  By default, all JTAG support is disabled but there NuttX
+  configuration options to enable JTAG in various different ways.
+
+  These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO
+  MAPR register.  These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the
+  Cortex debug port.  The default state in this port is for all JTAG support
+  to be disable.
+
+  CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full
+    SWJ (JTAG-DP + SW-DP)
+
+  CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable
+    full SWJ (JTAG-DP + SW-DP) but without JNTRST.
+
+  CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP
+    disabled and SW-DP enabled
+
+  The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100
+  which disable JTAG-DP and SW-DP.
+
+OpenOCD
+=======
+
+I have also used OpenOCD with the STM3210E-EVAL.  In this case, I used
+the Olimex USB ARM OCD.  See the script in configs/stm3210e-eval/tools/oocd.sh
+for more information.  Using the script:
+
+1) Start the OpenOCD GDB server
+
+   cd <nuttx-build-directory>
+   configs/stm3210e-eval/tools/oocd.sh $PWD
+
+2) Load Nuttx
+
+   cd <nuttx-built-directory>
+   arm-none-eabi-gdb nuttx
+   gdb> target remote localhost:3333
+   gdb> mon reset
+   gdb> mon halt
+   gdb> load nuttx
+
+3) Running NuttX
+
+   gdb> mon reset
+   gdb> c
+
+LEDs
+====
+
+The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the
+board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is
+defined.  In that case, the usage by the board port is defined in
+include/board.h and src/up_leds.c. The LEDs are used to encode OS-related
+events as follows:
+
+    SYMBOL           Meaning                 LED1* LED2  LED3  LED4
+    ---------------- ----------------------- ----- ----- ----- -----
+    LED_STARTED      NuttX has been started  ON    OFF   OFF   OFF
+    LED_HEAPALLOCATE Heap has been allocated OFF   ON    OFF   OFF
+    LED_IRQSENABLED  Interrupts enabled      ON    ON    OFF   OFF
+    LED_STACKCREATED Idle stack created      OFF   OFF   ON    OFF
+    LED_INIRQ        In an interrupt**       ON    N/C   N/C   OFF
+    LED_SIGNAL       In a signal handler***  N/C   ON    N/C   OFF
+    LED_ASSERTION    An assertion failed     ON    ON    N/C   OFF
+    LED_PANIC        The system has crashed  N/C   N/C   N/C   ON
+    LED_IDLE         STM32 is is sleep mode  (Optional, not used)
+
+  * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot
+    and these LEDs will give you some indication of where the failure was
+ ** The normal state is LED3 ON and LED1 faintly glowing.  This faint glow
+    is because of timer interupts that result in the LED being illuminated
+    on a small proportion of the time.
+*** LED2 may also flicker normally if signals are processed.
+
+Temperature Sensor
+==================
+
+Support for the on-board LM-75 temperature sensor is available.  This supported
+has been verified, but has not been included in any of the available the
+configurations.  To set up the temperature sensor, add the following to the
+NuttX configuration file
+
+  CONFIG_I2C=y
+  CONFIG_I2C_LM75=y
+
+Then you can implement logic like the following to use the temperature sensor:
+
+  #include <nuttx/sensors/lm75.h>
+  #include <arch/board/board.h>
+
+  ret =  stm32_lm75initialize("/dev/temp");       /* Register the temperature sensor */
+  fd = open("/dev/temp", O_RDONLY);               /* Open the temperature sensor device */
+  ret = ioctl(fd, SNIOC_FAHRENHEIT, 0);           /* Select Fahrenheit */
+  bytesread = read(fd, buffer, 8*sizeof(b16_t));  /* Read temperature samples */
+
+More complex temperature sensor operations are also available.  See the IOCTAL
+commands enumerated in include/nuttx/sensors/lm75.h.  Also read the descriptions
+of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the
+arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h).
+
+RTC
+===
+
+  The STM32 RTC may configured using the following settings.
+
+    CONFIG_RTC - Enables general support for a hardware RTC. Specific
+      architectures may require other specific settings.
+    CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1
+      second, usually supporting a 32-bit time_t value.  In this case,
+      the RTC is used to &quot;seed&quot; the normal NuttX timer and the
+      NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES
+      is enabled in the NuttX configuration, then the RTC provides higher
+      resolution time and completely replaces the system timer for purpose of
+      date and time.
+      CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the
+      frequency of the high resolution RTC must be provided.  If CONFIG_RTC_HIRES
+      is not defined, CONFIG_RTC_FREQUENCY is assumed to be one.
+    CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. 
+      A callback function will be executed when the alarm goes off
+
+  In hi-res mode, the STM32 RTC operates only at 16384Hz.  Overflow interrupts
+  are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes.
+  A BKP register is incremented on each overflow interrupt creating, effectively,
+  a 48-bit RTC counter.
+
+  In the lo-res mode, the RTC operates at 1Hz.  Overflow interrupts are not handled
+  (because the next overflow is not expected until the year 2106.
+
+   WARNING:  Overflow interrupts are lost whenever the STM32 is powered down.  The
+   overflow interrupt may be lost even if the STM32 is powered down only momentarily.
+   Therefore hi-res solution is only useful in systems where the power is always on.
+
+STM3210E-EVAL-specific Configuration Options
+============================================
+
+    CONFIG_ARCH - Identifies the arch/ subdirectory.  This should
+       be set to:
+
+       CONFIG_ARCH=arm
+
+    CONFIG_ARCH_family - For use in C code:
+
+       CONFIG_ARCH_ARM=y
+
+    CONFIG_ARCH_architecture - For use in C code:
+
+       CONFIG_ARCH_CORTEXM3=y
+
+    CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+
+       CONFIG_ARCH_CHIP=stm32
+
+    CONFIG_ARCH_CHIP_name - For use in C code to identify the exact
+       chip:
+
+       CONFIG_ARCH_CHIP_STM32F103ZET6
+
+    CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock
+       configuration features.
+
+       CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n
+ 
+    CONFIG_ARCH_BOARD - Identifies the configs subdirectory and
+       hence, the board that supports the particular chip or SoC.
+
+       CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board)
+
+    CONFIG_ARCH_BOARD_name - For use in C code
+
+       CONFIG_ARCH_BOARD_STM3210E_EVAL=y
+
+    CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation
+       of delay loops
+
+    CONFIG_ENDIAN_BIG - define if big endian (default is little
+       endian)
+
+    CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case):
+
+       CONFIG_DRAM_SIZE=0x00010000 (64Kb)
+
+    CONFIG_DRAM_START - The start address of installed DRAM
+
+       CONFIG_DRAM_START=0x20000000
+
+    CONFIG_DRAM_END - Last address+1 of installed RAM
+
+       CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE)
+
+    CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization
+
+       CONFIG_ARCH_IRQPRIO=y
+
+    CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that
+       have LEDs
+
+    CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+       stack. If defined, this symbol is the size of the interrupt
+        stack in bytes.  If not defined, the user task stacks will be
+      used during interrupt handling.
+
+    CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+
+    CONFIG_ARCH_LEDS -  Use LEDs to show state. Unique to board architecture.
+
+    CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+       cause a 100 second delay during boot-up.  This 100 second delay
+       serves no purpose other than it allows you to calibratre
+       CONFIG_ARCH_LOOPSPERMSEC.  You simply use a stop watch to measure
+       the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until
+       the delay actually is 100 seconds.
+
+  Individual subsystems can be enabled:
+    AHB
+    ---
+    CONFIG_STM32_DMA1
+    CONFIG_STM32_DMA2
+    CONFIG_STM32_CRC
+    CONFIG_STM32_FSMC
+    CONFIG_STM32_SDIO
+
+    APB1
+    ----
+    CONFIG_STM32_TIM2
+    CONFIG_STM32_TIM3
+    CONFIG_STM32_TIM4
+    CONFIG_STM32_TIM5
+    CONFIG_STM32_TIM6
+    CONFIG_STM32_TIM7
+    CONFIG_STM32_WWDG
+    CONFIG_STM32_SPI2
+    CONFIG_STM32_SPI4
+    CONFIG_STM32_USART2
+    CONFIG_STM32_USART3
+    CONFIG_STM32_UART4
+    CONFIG_STM32_UART5
+    CONFIG_STM32_I2C1
+    CONFIG_STM32_I2C2
+    CONFIG_STM32_USB
+    CONFIG_STM32_CAN
+    CONFIG_STM32_BKP
+    CONFIG_STM32_PWR
+    CONFIG_STM32_DAC1
+    CONFIG_STM32_DAC2
+    CONFIG_STM32_USB
+
+    APB2
+    ----
+    CONFIG_STM32_ADC1
+    CONFIG_STM32_ADC2
+    CONFIG_STM32_TIM1
+    CONFIG_STM32_SPI1
+    CONFIG_STM32_TIM8
+    CONFIG_STM32_USART1
+    CONFIG_STM32_ADC3
+
+  Timer and I2C devices may need to the following to force power to be applied
+  unconditionally at power up.  (Otherwise, the device is powered when it is
+  initialized).
+
+    CONFIG_STM32_FORCEPOWER
+
+  Timer devices may be used for different purposes.  One special purpose is
+  to generate modulated outputs for such things as motor control.  If CONFIG_STM32_TIMn
+  is defined (as above) then the following may also be defined to indicate that
+  the timer is intended to be used for pulsed output modulation, ADC conversion,
+  or DAC conversion.  Note that ADC/DAC require two definition:  Not only do you have
+  to assign the timer (n) for used by the ADC or DAC, but then you also have to
+  configure which ADC or DAC (m) it is assigned to.
+
+    CONFIG_STM32_TIMn_PWM   Reserve timer n for use by PWM, n=1,..,8
+    CONFIG_STM32_TIMn_ADC   Reserve timer n for use by ADC, n=1,..,8
+    CONFIG_STM32_TIMn_ADCm  Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3
+    CONFIG_STM32_TIMn_DAC   Reserve timer n for use by DAC, n=1,..,8
+    CONFIG_STM32_TIMn_DACm  Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2
+
+  For each timer that is enabled for PWM usage, we need the following additional
+  configuration settings:
+
+    CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4}
+ 
+  NOTE: The STM32 timers are each capable of generating different signals on
+  each of the four channels with different duty cycles.  That capability is
+  not supported by this driver:  Only one output channel per timer.
+
+  Alternate pin mappings (should not be used with the STM3210E-EVAL board):
+
+    CONFIG_STM32_TIM1_FULL_REMAP
+    CONFIG_STM32_TIM1_PARTIAL_REMAP
+    CONFIG_STM32_TIM2_FULL_REMAP
+    CONFIG_STM32_TIM2_PARTIAL_REMAP_1
+    CONFIG_STM32_TIM2_PARTIAL_REMAP_2
+    CONFIG_STM32_TIM3_FULL_REMAP
+    CONFIG_STM32_TIM3_PARTIAL_REMAP
+    CONFIG_STM32_TIM4_REMAP
+    CONFIG_STM32_USART1_REMAP
+    CONFIG_STM32_USART2_REMAP
+    CONFIG_STM32_USART3_FULL_REMAP
+    CONFIG_STM32_USART3_PARTIAL_REMAP
+    CONFIG_STM32_SPI1_REMAP
+    CONFIG_STM32_SPI3_REMAP
+    CONFIG_STM32_I2C1_REMAP
+    CONFIG_STM32_CAN1_FULL_REMAP
+    CONFIG_STM32_CAN1_PARTIAL_REMAP
+    CONFIG_STM32_CAN2_REMAP
+
+  JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
+    CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+    CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+      but without JNTRST.
+    CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
+
+  STM32F103Z specific device driver settings
+
+    CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART
+           m (m=4,5) for the console and ttys0 (default is the USART1).
+    CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received.
+       This specific the size of the receive buffer
+    CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before
+       being sent.  This specific the size of the transmit buffer
+    CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART.  Must be
+    CONFIG_U[S]ARTn_BITS - The number of bits.  Must be either 7 or 8.
+    CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+    CONFIG_U[S]ARTn_2STOP - Two stop bits
+
+    CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI
+      support. Non-interrupt-driven, poll-waiting is recommended if the
+      interrupt rate would be to high in the interrupt driven case.
+    CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance.
+      Cannot be used with CONFIG_STM32_SPI_INTERRUPT.
+
+    CONFIG_SDIO_DMA - Support DMA data transfers.  Requires CONFIG_STM32_SDIO
+      and CONFIG_STM32_DMA2.
+    CONFIG_SDIO_PRI - Select SDIO interrupt prority.  Default: 128
+    CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. 
+      Default:  Medium
+    CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode.  Default:
+      4-bit transfer mode.
+
+  STM3210E-EVAL CAN Configuration
+
+    CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or
+      CONFIG_STM32_CAN2 must also be defined)
+    CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID.  Default
+      Standard 11-bit IDs.
+    CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages.
+      Default: 8
+    CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests.
+      Default: 4
+    CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback
+      mode for testing. The STM32 CAN driver does support loopback mode.
+    CONFIG_CAN1_BAUD - CAN1 BAUD rate.  Required if CONFIG_STM32_CAN1 is defined.
+    CONFIG_CAN2_BAUD - CAN1 BAUD rate.  Required if CONFIG_STM32_CAN2 is defined.
+    CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6
+    CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7
+    CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an
+      dump of all CAN registers.
+
+  STM3210E-EVAL LCD Hardware Configuration
+
+    CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape"
+      support. Default is this 320x240 "landscape" orientation
+      (this setting is informative only... not used).
+    CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait"
+      orientation support.  In this orientation, the STM3210E-EVAL's
+      LCD ribbon cable is at the bottom of the display. Default is
+      320x240 "landscape" orientation.
+    CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse
+      portrait" orientation support.  In this orientation, the
+      STM3210E-EVAL's LCD ribbon cable is at the top of the display.
+      Default is 320x240 "landscape" orientation.
+    CONFIG_LCD_BACKLIGHT - Define to support a backlight.
+    CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an
+      adjustable backlight will be provided using timer 1 to generate
+      various pulse widthes.  The granularity of the settings is
+      determined by CONFIG_LCD_MAXPOWER.  If CONFIG_LCD_PWM (or
+      CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight
+      is provided.
+    CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears
+      to be a shift in the returned data.  This value fixes the offset.
+      Default 5.
+
+    The LCD driver dynamically selects the LCD based on the reported LCD
+    ID value.  However, code size can be reduced by suppressing support for
+    individual LCDs using:
+
+    CONFIG_STM32_AM240320_DISABLE
+    CONFIG_STM32_SPFD5408B_DISABLE
+    CONFIG_STM32_R61580_DISABLE
+
+Configurations
+==============
+
+Each STM3210E-EVAL configuration is maintained in a sudirectory and
+can be selected as follow:
+
+    cd tools
+    ./configure.sh stm3210e-eval/<subdir>
+    cd -
+    . ./setenv.sh
+
+Where <subdir> is one of the following:
+
+  buttons:
+  --------
+
+    Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and
+    button interrupts.
+ 
+    CONFIG_STM32_CODESOURCERYW=y  : CodeSourcery under Windows
+
+  composite
+  ---------
+
+    This configuration exercises a composite USB interface consisting
+    of a CDC/ACM device and a USB mass storage device.  This configuration
+    uses apps/examples/composite.
+
+  nsh and nsh2:
+  ------------
+    Configure the NuttShell (nsh) located at examples/nsh.
+
+    Differences between the two NSH configurations:
+
+    =========== ======================= ================================
+                nsh                     nsh2
+    =========== ======================= ================================
+    Toolchain:  NuttX buildroot for     Codesourcery for Windows (1)
+                Linux or Cygwin (1,2)
+    ----------- ----------------------- --------------------------------
+    Loader:     DfuSe                   DfuSe
+    ----------- ----------------------- --------------------------------
+    Serial      Debug output: USART1    Debug output: USART1
+    Console:    NSH output:   USART1    NSH output:   USART1 (3)
+    ----------- ----------------------- --------------------------------
+    microSD     Yes                     Yes
+    Support
+    ----------- ----------------------- --------------------------------
+    FAT FS      CONFIG_FAT_LCNAME=y     CONFIG_FAT_LCNAME=y
+    Config      CONFIG_FAT_LFN=n        CONFIG_FAT_LFN=y (4)
+    ----------- ----------------------- --------------------------------
+    Support for No                      Yes
+    Built-in
+    Apps
+    ----------- ----------------------- --------------------------------
+    Built-in    None                    apps/examples/nx
+    Apps                                apps/examples/nxhello
+                                        apps/examples/usbstorage (5)
+    =========== ======================= ================================
+
+    (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh
+        to set up the correct PATH variable for whichever toolchain you
+        may use.
+    (2) Since DfuSe is assumed, this configuration may only work under
+        Cygwin without modification.
+    (3) When any other device other than /dev/console is used for a user
+        interface, (1) linefeeds (\n) will not be expanded to carriage return
+        / linefeeds \r\n). You will need to configure your terminal program
+        to account for this. And (2) input is not automatically echoed so
+        you will have to turn local echo on.
+    (4) Microsoft holds several patents related to the design of
+        long file names in the FAT file system.  Please refer to the
+        details in the top-level COPYING file.  Please do not use FAT
+        long file name unless you are familiar with these patent issues.
+    (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y),
+        Caution should be used to assure that the SD drive is not in use when
+        the USB storage device is configured.  Specifically, the SD driver
+        should be unmounted like:
+
+        nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH
+        ...
+        nsh> umount /mnd/sdcard                    # Unmount before connecting USB!!!
+        nsh> msconn                                # Connect the USB storage device
+        ...
+        nsh> msdis                                 # Disconnect USB storate device
+        nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount
+
+        Failure to do this could result in corruption of the SD card format.
+
+  nx:
+  ---
+    An example using the NuttX graphics system (NX).  This example
+    focuses on general window controls, movement, mouse and keyboard
+    input.
+
+    CONFIG_STM32_CODESOURCERYW=y  : CodeSourcery under Windows
+    CONFIG_LCD_RPORTRAIT=y        : 240x320 reverse portrait
+
+  nxlines:
+  ------
+    Another example using the NuttX graphics system (NX).   This
+    example focuses on placing lines on the background in various
+    orientations.
+
+    CONFIG_STM32_CODESOURCERYW=y  : CodeSourcery under Windows
+    CONFIG_LCD_RPORTRAIT=y        : 240x320 reverse portrait
+
+  nxtext:
+  ------
+    Another example using the NuttX graphics system (NX).   This
+    example focuses on placing text on the background while pop-up
+    windows occur.  Text should continue to update normally with
+    or without the popup windows present.
+
+    CONFIG_STM32_BUILDROOT=y      : NuttX buildroot under Linux or Cygwin
+    CONFIG_LCD_RPORTRAIT=y        : 240x320 reverse portrait
+
+    NOTE:  When I tried building this example with the CodeSourcery
+    tools, I got a hardfault inside of its libgcc.  I haven't
+    retested since then, but beware if you choose to change the
+    toolchain.
+
+  ostest:
+  ------
+    This configuration directory, performs a simple OS test using
+    examples/ostest.  By default, this project assumes that you are
+    using the DFU bootloader.
+
+    CONFIG_STM32_BUILDROOT=y      : NuttX buildroot under Linux or Cygwin
+
+  RIDE
+  ----
+    This configuration builds a trivial bring-up binary.  It is
+    useful only because it words with the RIDE7 IDE and R-Link debugger.
+
+    CONFIG_STM32_RAISONANCE=y     : Raisonance RIDE7 under Windows
+
+  usbserial:
+  ---------
+    This configuration directory exercises the USB serial class
+    driver at examples/usbserial.  See examples/README.txt for
+    more information.
+
+    CONFIG_STM32_BUILDROOT=y      : NuttX buildroot under Linux or Cygwin
+
+    USB debug output can be enabled as by changing the following
+    settings in the configuration file:
+
+    -CONFIG_DEBUG=n
+    -CONFIG_DEBUG_VERBOSE=n
+    -CONFIG_DEBUG_USB=n
+    +CONFIG_DEBUG=y
+    +CONFIG_DEBUG_VERBOSE=y
+    +CONFIG_DEBUG_USB=y
+
+    -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n
+    -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n
+    -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n
+    -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n
+    -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n
+    +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y
+    +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y
+    +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y
+    +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y
+    +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y
+
+    By default, the usbserial example uses the Prolific PL2303
+    serial/USB converter emulation.  The example can be modified
+    to use the CDC/ACM serial class by making the following changes
+    to the configuration file:
+
+    -CONFIG_PL2303=y
+    +CONFIG_PL2303=n
+
+    -CONFIG_CDCACM=n
+    +CONFIG_CDCACM=y
+
+    The example can also be converted to use the alternative
+    USB serial example at apps/examples/usbterm by changing the 
+    following:
+
+    -CONFIGURED_APPS += examples/usbserial
+    +CONFIGURED_APPS += examples/usbterm
+
+    In either the original appconfig file (before configuring)
+    or in the final apps/.config file (after configuring).
+
+  usbstorage:
+  ----------
+    This configuration directory exercises the USB mass storage
+    class driver at examples/usbstorage.  See examples/README.txt for
+    more information.
+
+    CONFIG_STM32_BUILDROOT=y      : NuttX buildroot under Linux or Cygwin
+
diff --git a/nuttx-configs/px4io-v1/common/Make.defs b/nuttx-configs/px4io-v1/common/Make.defs
new file mode 100644
index 000000000..74b183067
--- /dev/null
+++ b/nuttx-configs/px4io-v1/common/Make.defs
@@ -0,0 +1,171 @@
+############################################################################
+# configs/px4fmu/common/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+#
+# Generic Make.defs for the PX4FMU
+# Do not specify/use this file directly - it is included by config-specific
+# Make.defs in the per-config directories.
+#
+
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -O3
+ARCHCPUFLAGS		 = -mcpu=cortex-m3 \
+			   -mthumb \
+			   -march=armv7-m
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain 
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+   			   -funsafe-math-optimizations \
+   			   -fno-builtin-printf \
+   			   -ffunction-sections \
+   			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = -Wall \
+			   -Wextra \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wfloat-equal \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wmissing-declarations \
+			   -Wpacked \
+			   -Wno-unused-parameter
+#   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
+#   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+#   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS		 = $(ARCHWARNINGS) \
+			   -Wbad-function-cast \
+			   -Wstrict-prototypes \
+			   -Wold-style-declaration \
+			   -Wmissing-parameter-type \
+			   -Wmissing-prototypes \
+			   -Wnested-externs \
+			   -Wunsuffixed-float-constants
+ARCHWARNINGSXX		 = $(ARCHWARNINGS)
+ARCHDEFINES		 = 
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+
diff --git a/nuttx-configs/px4io-v1/common/ld.script b/nuttx-configs/px4io-v1/common/ld.script
new file mode 100755
index 000000000..69c2f9cb2
--- /dev/null
+++ b/nuttx-configs/px4io-v1/common/ld.script
@@ -0,0 +1,129 @@
+/****************************************************************************
+ * configs/stm3210e-eval/nsh/ld.script
+ *
+ *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
+ * 8Kb of SRAM beginning at address 0x2000:0000.  When booting from FLASH,
+ * FLASH memory is aliased to address 0x0000:0000 where the code expects to
+ * begin execution by jumping to the entry point in the 0x0800:0000 address
+ * range.
+ */
+
+MEMORY
+{
+    flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
+    sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
+}
+
+OUTPUT_ARCH(arm)
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/* 
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)        
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)        
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	/* The STM32F100CB has 8Kb of SRAM beginning at the following address */
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/px4io-v1/common/setenv.sh b/nuttx-configs/px4io-v1/common/setenv.sh
new file mode 100755
index 000000000..ff9a4bf8a
--- /dev/null
+++ b/nuttx-configs/px4io-v1/common/setenv.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+# configs/stm3210e-eval/dfu/setenv.sh
+#
+#   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$(basename $0)" = "setenv.sh" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi
+
+WD=`pwd`
+export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin"
+export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4io-v1/include/README.txt b/nuttx-configs/px4io-v1/include/README.txt
new file mode 100755
index 000000000..2264a80aa
--- /dev/null
+++ b/nuttx-configs/px4io-v1/include/README.txt
@@ -0,0 +1 @@
+This directory contains header files unique to the PX4IO board.
diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h
new file mode 100755
index 000000000..815079266
--- /dev/null
+++ b/nuttx-configs/px4io-v1/include/board.h
@@ -0,0 +1,152 @@
+/************************************************************************************
+ * configs/px4io/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   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 NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+# include <stdbool.h>
+#endif
+#include <stm32_rcc.h>
+#include <stm32_sdio.h>
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+
+/* On-board crystal frequency is 24MHz (HSE) */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+/* Use the HSE output as the system clock */
+
+#define STM32_SYSCLK_SW         RCC_CFGR_SW_HSE
+#define STM32_SYSCLK_SWS        RCC_CFGR_SWS_HSE
+#define STM32_SYSCLK_FREQUENCY  STM32_BOARD_XTAL
+
+/* AHB clock (HCLK) is SYSCLK (24MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY    /* same as above, to satisfy compiler */
+
+/* APB2 clock (PCLK2) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLK
+#define STM32_PCLK2_FREQUENCY   STM32_HCLK_FREQUENCY
+#define STM32_APB2_CLKIN        (STM32_PCLK2_FREQUENCY)   /* Timers 2-4 */
+
+/* APB2 timer 1 will receive PCLK2. */
+
+#define STM32_APB2_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (STM32_PCLK2_FREQUENCY)
+
+/* APB1 clock (PCLK1) is HCLK (24MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLK
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY)
+
+/* All timers run off PCLK */
+
+#define STM32_APB1_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB1_TIM2_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (STM32_PCLK1_FREQUENCY)
+
+/*
+ * Some of the USART pins are not available; override the GPIO
+ * definitions with an invalid pin configuration.
+ */
+#undef GPIO_USART2_CTS
+#define GPIO_USART2_CTS	0xffffffff
+#undef GPIO_USART2_RTS
+#define GPIO_USART2_RTS	0xffffffff
+#undef GPIO_USART2_CK
+#define GPIO_USART2_CK 	0xffffffff
+#undef GPIO_USART3_TX
+#define GPIO_USART3_TX 	0xffffffff
+#undef GPIO_USART3_CK
+#define GPIO_USART3_CK 	0xffffffff
+#undef GPIO_USART3_CTS
+#define GPIO_USART3_CTS	0xffffffff
+#undef GPIO_USART3_RTS
+#define GPIO_USART3_RTS	0xffffffff
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+    
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx-configs/px4io-v1/include/drv_i2c_device.h b/nuttx-configs/px4io-v1/include/drv_i2c_device.h
new file mode 100644
index 000000000..02582bc09
--- /dev/null
+++ b/nuttx-configs/px4io-v1/include/drv_i2c_device.h
@@ -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.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file A simple, polled I2C slave-mode driver.
+  *
+  * The master writes to and reads from a byte buffer, which the caller 
+  * can update inbetween calls to the FSM.
+  */
+
+extern void	i2c_fsm_init(uint8_t *buffer, size_t buffer_size);
+extern bool	i2c_fsm(void);
diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs
new file mode 100644
index 000000000..c7f6effd9
--- /dev/null
+++ b/nuttx-configs/px4io-v1/nsh/Make.defs
@@ -0,0 +1,3 @@
+include ${TOPDIR}/.config
+
+include $(TOPDIR)/configs/px4io-v1/common/Make.defs
diff --git a/nuttx-configs/px4io-v1/nsh/appconfig b/nuttx-configs/px4io-v1/nsh/appconfig
new file mode 100644
index 000000000..48a41bcdb
--- /dev/null
+++ b/nuttx-configs/px4io-v1/nsh/appconfig
@@ -0,0 +1,32 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig
new file mode 100755
index 000000000..525672233
--- /dev/null
+++ b/nuttx-configs/px4io-v1/nsh/defconfig
@@ -0,0 +1,559 @@
+############################################################################
+# configs/px4io-v1/nsh/defconfig
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+#
+# architecture selection
+#
+# CONFIG_ARCH - identifies the arch subdirectory and, hence, the
+#   processor architecture.
+# CONFIG_ARCH_family - for use in C code.  This identifies the
+#   particular chip family that the architecture is implemented
+#   in.
+# CONFIG_ARCH_architecture - for use in C code.  This identifies the
+#   specific architecture within the chip family.
+# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory
+# CONFIG_ARCH_CHIP_name - For use in C code
+# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence,
+#   the board that supports the particular chip or SoC.
+# CONFIG_ARCH_BOARD_name - for use in C code
+# CONFIG_ENDIAN_BIG - define if big endian (default is little endian)
+# CONFIG_BOARD_LOOPSPERMSEC - for delay loops
+# CONFIG_DRAM_SIZE - Describes the installed DRAM.
+# CONFIG_DRAM_START - The start address of DRAM (physical)
+# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization
+# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt
+#   stack. If defined, this symbol is the size of the interrupt
+#   stack in bytes.  If not defined, the user task stacks will be
+#   used during interrupt handling.
+# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions
+# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader.
+# CONFIG_ARCH_LEDS -  Use LEDs to show state. Unique to board architecture.
+# CONFIG_ARCH_BUTTONS -  Enable support for buttons. Unique to board architecture.
+# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that
+#   cause a 100 second delay during boot-up.  This 100 second delay
+#   serves no purpose other than it allows you to calibrate
+#   CONFIG_BOARD_LOOPSPERMSEC.  You simply use a stop watch to measure
+#   the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until
+#   the delay actually is 100 seconds.
+# CONFIG_ARCH_DMA - Support DMA initialization
+#
+CONFIG_ARCH="arm"
+CONFIG_ARCH_ARM=y
+CONFIG_ARCH_CORTEXM3=y
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32F100C8=y
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_PX4IO_V1=y
+# CONFIG_ARCH_BOARD_CUSTOM is not set
+CONFIG_ARCH_BOARD="px4io-v1"
+CONFIG_BOARD_LOOPSPERMSEC=2000
+CONFIG_DRAM_SIZE=0x00002000
+CONFIG_DRAM_START=0x20000000
+CONFIG_ARCH_IRQPRIO=y
+CONFIG_ARCH_INTERRUPTSTACK=n
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARCH_BOOTLOADER=n
+CONFIG_ARCH_LEDS=n
+CONFIG_ARCH_BUTTONS=n
+CONFIG_ARCH_CALIBRATION=n
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_MATH_H=y
+
+CONFIG_ARMV7M_CMNVECTOR=y
+# CONFIG_ARMV7M_STACKCHECK is not set
+
+#
+# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
+#
+#   CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG
+#
+# JTAG Enable options:
+#
+#   CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+#   CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP)
+#     but without JNTRST.
+#   CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
+#
+CONFIG_STM32_DFU=n
+CONFIG_STM32_JTAG_FULL_ENABLE=y
+CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=n
+
+#
+# Individual subsystems can be enabled:
+#
+# AHB: 
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=n
+CONFIG_STM32_CRC=n
+# APB1:
+# Timers 2,3 and 4 are owned by the PWM driver
+CONFIG_STM32_TIM2=n
+CONFIG_STM32_TIM3=n
+CONFIG_STM32_TIM4=n
+CONFIG_STM32_TIM5=n
+CONFIG_STM32_TIM6=n
+CONFIG_STM32_TIM7=n
+CONFIG_STM32_WWDG=n
+CONFIG_STM32_SPI2=n
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_I2C2=n
+CONFIG_STM32_BKP=n
+CONFIG_STM32_PWR=n
+CONFIG_STM32_DAC=n
+# APB2:
+# We use our own ADC driver, but leave this on for clocking purposes.
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_ADC2=n
+# TIM1 is owned by the HRT
+CONFIG_STM32_TIM1=n
+CONFIG_STM32_SPI1=n
+CONFIG_STM32_TIM8=n
+CONFIG_STM32_USART1=y
+CONFIG_STM32_ADC3=n
+
+
+#
+# STM32F100 specific serial device driver settings
+#
+# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the
+#   console and ttys0 (default is the USART1).
+# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received.
+#   This specific the size of the receive buffer
+# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before
+#   being sent.  This specific the size of the transmit buffer
+# CONFIG_USARTn_BAUD - The configure BAUD of the UART.  Must be
+# CONFIG_USARTn_BITS - The number of bits.  Must be either 7 or 8.
+# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity
+# CONFIG_USARTn_2STOP - Two stop bits
+#
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_STANDARD_SERIAL=y
+
+CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART2_SERIAL_CONSOLE=n
+CONFIG_USART3_SERIAL_CONSOLE=n
+
+CONFIG_USART1_TXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART3_TXBUFSIZE=64
+
+CONFIG_USART1_RXBUFSIZE=64
+CONFIG_USART2_RXBUFSIZE=64
+CONFIG_USART3_RXBUFSIZE=64
+
+CONFIG_USART1_BAUD=115200
+CONFIG_USART2_BAUD=115200
+CONFIG_USART3_BAUD=115200
+
+CONFIG_USART1_BITS=8
+CONFIG_USART2_BITS=8
+CONFIG_USART3_BITS=8
+
+CONFIG_USART1_PARITY=0
+CONFIG_USART2_PARITY=0
+CONFIG_USART3_PARITY=0
+
+CONFIG_USART1_2STOP=0
+CONFIG_USART2_2STOP=0
+CONFIG_USART3_2STOP=0
+
+CONFIG_USART1_RXDMA=y
+SERIAL_HAVE_CONSOLE_DMA=y
+# Conflicts with I2C1 DMA
+CONFIG_USART2_RXDMA=n
+CONFIG_USART3_RXDMA=y
+
+#
+# PX4IO specific driver settings
+#
+# CONFIG_HRT_TIMER
+#   Enables the high-resolution timer.  The board definition must
+#   set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/
+#   compare channels to be used.
+# CONFIG_HRT_PPM
+#   Enables R/C PPM input using the HRT.  The board definition must
+#   set HRT_PPM_CHANNEL to the timer capture/compare channel to be
+#   used, and define GPIO_PPM_IN to configure the appropriate timer
+#   GPIO.
+# CONFIG_PWM_SERVO
+#   Enables the PWM servo driver.  The driver configuration must be
+#   supplied by the board support at initialisation time.
+#   Note that USART2 must be disabled on the PX4 board for this to
+#   be available.
+#
+#
+CONFIG_HRT_TIMER=y
+CONFIG_HRT_PPM=y
+
+#
+# General build options
+#
+# CONFIG_RRLOAD_BINARY - make the rrload binary format used with
+#   BSPs from www.ridgerun.com using the tools/mkimage.sh script
+# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format
+#   used with many different loaders using the GNU objcopy program
+#   Should not be selected if you are not using the GNU toolchain.
+# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format
+#   used with many different loaders using the GNU objcopy program
+#   Should not be selected if you are not using the GNU toolchain.
+# CONFIG_RAW_BINARY - make a raw binary format file used with many
+#   different loaders using the GNU objcopy program.  This option
+#   should not be selected if you are not using the GNU toolchain.
+# CONFIG_HAVE_LIBM - toolchain supports libm.a
+#
+CONFIG_RRLOAD_BINARY=n
+CONFIG_INTELHEX_BINARY=n
+CONFIG_MOTOROLA_SREC=n
+CONFIG_RAW_BINARY=y
+CONFIG_HAVE_LIBM=n
+
+#
+# General OS setup
+#
+# CONFIG_APPS_DIR - Identifies the relative path to the directory
+#   that builds the application to link with NuttX.  Default: ../apps
+# CONFIG_DEBUG - enables built-in debug options
+# CONFIG_DEBUG_VERBOSE - enables verbose debug output
+# CONFIG_DEBUG_SYMBOLS - build without optimization and with
+#   debug symbols (needed for use with a debugger).
+# CONFIG_HAVE_CXX - Enable support for C++
+# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support
+#   for initialization of static C++ instances for this architecture
+#   and for the selected toolchain (via up_cxxinitialize()).
+# CONFIG_MM_REGIONS - If the architecture includes multiple
+#   regions of memory to allocate from, this specifies the
+#   number of memory regions that the memory manager must
+#   handle and enables the API mm_addregion(start, end);
+# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot
+#   time console output
+# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz
+#   or MSEC_PER_TICK=10.  This setting may be defined to
+#   inform NuttX that the processor hardware is providing
+#   system timer interrupts at some interrupt interval other
+#   than 10 msec.
+# CONFIG_RR_INTERVAL - The round robin timeslice will be set
+#   this number of milliseconds;  Round robin scheduling can
+#   be disabled by setting this value to zero.
+# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in 
+#   scheduler to monitor system performance
+# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a
+#   task name to save in the TCB.  Useful if scheduler
+#   instrumentation is selected.  Set to zero to disable.
+# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY -
+#   Used to initialize the internal time logic.
+# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions.
+#   You would only need this if you are concerned about accurate
+#   time conversions in the past or in the distant future.
+# CONFIG_JULIAN_TIME - Enables Julian time conversions. You
+#   would only need this if you are concerned about accurate
+#   time conversion in the distand past.  You must also define
+#   CONFIG_GREGORIAN_TIME in order to use Julian time.
+# CONFIG_DEV_CONSOLE - Set if architecture-specific logic
+#   provides /dev/console.  Enables stdout, stderr, stdin.
+# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console
+#   driver (minimul support)
+# CONFIG_MUTEX_TYPES: Set to enable support for recursive and
+#   errorcheck mutexes. Enables pthread_mutexattr_settype().
+# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority
+#   inheritance on mutexes and semaphores. 
+# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority
+#   inheritance is enabled.  It defines the maximum number of
+#   different threads (minus one) that can take counts on a
+#   semaphore with priority inheritance support.  This may be 
+#   set to zero if priority inheritance is disabled OR if you
+#   are only using semaphores as mutexes (only one holder) OR
+#   if no more than two threads participate using a counting
+#   semaphore.
+# CONFIG_SEM_NNESTPRIO.  If priority inheritance is enabled,
+#   then this setting is the maximum number of higher priority
+#   threads (minus 1) than can be waiting for another thread
+#   to release a count on a semaphore.  This value may be set
+#   to zero if no more than one thread is expected to wait for
+#   a semaphore.
+# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors
+#   by task_create() when a new task is started.  If set, all
+#   files/drivers will appear to be closed in the new task.
+# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first
+#   three file descriptors (stdin, stdout, stderr) by task_create()
+#   when a new task is started. If set, all files/drivers will
+#   appear to be closed in the new task except for stdin, stdout,
+#   and stderr.
+# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket
+#   desciptors by task_create() when a new task is started. If
+#   set, all sockets will appear to be closed in the new task.
+# CONFIG_SCHED_WORKQUEUE.  Create a dedicated "worker" thread to
+#  handle delayed processing from interrupt handlers.  This feature
+#  is required for some drivers but, if there are not complaints,
+#  can be safely disabled.  The worker thread also performs
+#  garbage collection -- completing any delayed memory deallocations
+#  from interrupt handlers.  If the worker thread is disabled,
+#  then that clean will be performed by the IDLE thread instead
+#  (which runs at the lowest of priority and may not be appropriate
+#  if memory reclamation is of high priority).  If CONFIG_SCHED_WORKQUEUE
+#  is enabled, then the following options can also be used:
+# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker
+#  thread.  Default: 50
+# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for
+#  work in units of microseconds.  Default: 50*1000 (50 MS).
+# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker
+#  thread.  Default: CONFIG_IDLETHREAD_STACKSIZE.
+# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up
+#  the worker thread.  Default: 4
+# CONFIG_SCHED_WAITPID - Enable the waitpid() API
+# CONFIG_SCHED_ATEXIT - Enabled the atexit() API
+#
+CONFIG_USER_ENTRYPOINT="user_start"
+#CONFIG_APPS_DIR=
+CONFIG_DEBUG=n
+CONFIG_DEBUG_VERBOSE=n
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_FS=n
+CONFIG_DEBUG_GRAPHICS=n
+CONFIG_DEBUG_LCD=n
+CONFIG_DEBUG_USB=n
+CONFIG_DEBUG_NET=n
+CONFIG_DEBUG_RTC=n
+CONFIG_DEBUG_ANALOG=n
+CONFIG_DEBUG_PWM=n
+CONFIG_DEBUG_CAN=n
+CONFIG_DEBUG_I2C=n
+CONFIG_DEBUG_INPUT=n
+
+CONFIG_MSEC_PER_TICK=1
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_MM_REGIONS=1
+CONFIG_MM_SMALL=y
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=n
+CONFIG_TASK_NAME_SIZE=8
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_GREGORIAN_TIME=n
+CONFIG_JULIAN_TIME=n
+# this eats ~1KiB of RAM ... work out why
+CONFIG_DEV_CONSOLE=y
+CONFIG_DEV_LOWCONSOLE=n
+CONFIG_MUTEX_TYPES=n
+CONFIG_PRIORITY_INHERITANCE=n
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=0
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WORKQUEUE=n
+CONFIG_SCHED_WORKPRIORITY=50
+CONFIG_SCHED_WORKPERIOD=50000
+CONFIG_SCHED_WORKSTACKSIZE=1024
+CONFIG_SIG_SIGWORK=4
+CONFIG_SCHED_WAITPID=n
+CONFIG_SCHED_ATEXIT=n
+
+#
+# The following can be used to disable categories of
+# APIs supported by the OS.  If the compiler supports
+# weak functions, then it should not be necessary to
+# disable functions unless you want to restrict usage
+# of those APIs.
+#
+# There are certain dependency relationships in these
+# features.
+#
+# o mq_notify logic depends on signals to awaken tasks
+#   waiting for queues to become full or empty.
+# o pthread_condtimedwait() depends on signals to wake
+#   up waiting tasks.
+#
+CONFIG_DISABLE_CLOCK=n
+CONFIG_DISABLE_POSIX_TIMERS=y
+CONFIG_DISABLE_PTHREAD=y
+CONFIG_DISABLE_SIGNALS=y
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_DISABLE_ENVIRON=y
+CONFIG_DISABLE_POLL=y
+
+#
+# Misc libc settings
+#
+# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a
+#   little smaller if we do not support fieldwidthes
+#
+CONFIG_NOPRINTF_FIELDWIDTH=n
+
+#
+# Allow for architecture optimized implementations
+#
+# The architecture can provide optimized versions of the
+# following to improve system performance
+#
+CONFIG_ARCH_MEMCPY=n
+CONFIG_ARCH_MEMCMP=n
+CONFIG_ARCH_MEMMOVE=n
+CONFIG_ARCH_MEMSET=n
+CONFIG_ARCH_STRCMP=n
+CONFIG_ARCH_STRCPY=n
+CONFIG_ARCH_STRNCPY=n
+CONFIG_ARCH_STRLEN=n
+CONFIG_ARCH_STRNLEN=n
+CONFIG_ARCH_BZERO=n
+
+#
+# Sizes of configurable things (0 disables)
+#
+# CONFIG_MAX_TASKS - The maximum number of simultaneously
+#   active tasks.  This value must be a power of two.
+# CONFIG_MAX_TASK_ARGS - This controls the maximum number of
+#   of parameters that a task may receive (i.e., maxmum value
+#   of 'argc')
+# CONFIG_NPTHREAD_KEYS - The number of items of thread-
+#   specific data that can be retained
+# CONFIG_NFILE_DESCRIPTORS - The maximum number of file
+#   descriptors (one for each open)
+# CONFIG_NFILE_STREAMS - The maximum number of streams that
+#   can be fopen'ed
+# CONFIG_NAME_MAX - The maximum size of a file name.
+# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate
+#   on fopen. (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled
+#   (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added
+#   to force automatic, line-oriented flushing the output buffer
+#   for putc(), fputc(), putchar(), puts(), fputs(), printf(),
+#   fprintf(), and vfprintf().  When a newline is encountered in
+#   the output string, the output buffer will be flushed.  This
+#   (slightly) increases the NuttX footprint but supports the kind
+#   of behavior that people expect for printf().
+# CONFIG_NUNGET_CHARS - Number of characters that can be
+#   buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0)
+# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message
+#   structures.  The system manages a pool of preallocated
+#   message structures to minimize dynamic allocations
+# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with
+#   a fixed payload size given by this settin (does not include
+#   other message structure overhead.
+# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that
+#   can be passed to a watchdog handler
+# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog
+#   structures.  The system manages a pool of preallocated
+#   watchdog structures to minimize dynamic allocations
+# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX
+#   timer structures.  The system manages a pool of preallocated
+#   timer structures to minimize dynamic allocations.  Set to
+#   zero for all dynamic allocations.
+#
+CONFIG_MAX_TASKS=4
+CONFIG_MAX_TASK_ARGS=4
+CONFIG_NPTHREAD_KEYS=2
+CONFIG_NFILE_DESCRIPTORS=8
+CONFIG_NFILE_STREAMS=0
+CONFIG_NAME_MAX=12
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=n
+CONFIG_NUNGET_CHARS=2
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=4
+CONFIG_PREALLOC_TIMERS=0
+
+
+#
+# Settings for apps/nshlib
+#
+# CONFIG_NSH_BUILTIN_APPS - Support external registered,
+#   "named" applications that can be executed from the NSH
+#   command line (see apps/README.txt for more information).
+# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
+# CONFIG_NSH_STRERROR - Use strerror(errno)
+# CONFIG_NSH_LINELEN - Maximum length of one command line
+# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
+# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
+# CONFIG_NSH_DISABLEBG - Disable background commands
+# CONFIG_NSH_ROMFSETC - Use startup script in /etc
+# CONFIG_NSH_CONSOLE - Use serial console front end
+# CONFIG_NSH_TELNET - Use telnetd console front end
+# CONFIG_NSH_ARCHINIT - Platform provides architecture
+#   specific initialization (nsh_archinitialize()).
+#
+
+# Disable NSH completely
+CONFIG_NSH_CONSOLE=n
+
+#
+# Stack and heap information
+#
+# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP
+#   operation from FLASH but must copy initialized .data sections to RAM.
+#   (should also be =n for the STM3210E-EVAL which always runs from flash)
+# CONFIG_BOOT_COPYTORAM -  Some configurations boot in FLASH
+#   but copy themselves entirely into RAM for better performance.
+# CONFIG_CUSTOM_STACK - The up_ implementation will handle
+#   all stack operations outside of the nuttx model.
+# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only)
+# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack.
+#  This is the thread that (1) performs the inital boot of the system up
+#  to the point where user_start() is spawned, and (2) there after is the
+#  IDLE thread that executes only when there is no other thread ready to
+#  run.
+# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate
+#  for the main user thread that begins at the user_start() entry point.
+# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size
+# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size
+# CONFIG_HEAP_BASE - The beginning of the heap
+# CONFIG_HEAP_SIZE - The size of the heap
+#
+CONFIG_BOOT_RUNFROMFLASH=n
+CONFIG_BOOT_COPYTORAM=n
+CONFIG_CUSTOM_STACK=n
+CONFIG_STACK_POINTER=
+CONFIG_IDLETHREAD_STACKSIZE=1024
+CONFIG_USERMAIN_STACKSIZE=1200
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=1024
+CONFIG_HEAP_BASE=
+CONFIG_HEAP_SIZE=
+
+#
+# NSH Library
+#
+# CONFIG_NSH_LIBRARY is not set
+# CONFIG_NSH_BUILTIN_APPS is not set
diff --git a/nuttx-configs/px4io-v1/nsh/setenv.sh b/nuttx-configs/px4io-v1/nsh/setenv.sh
new file mode 100755
index 000000000..ff9a4bf8a
--- /dev/null
+++ b/nuttx-configs/px4io-v1/nsh/setenv.sh
@@ -0,0 +1,47 @@
+#!/bin/bash
+# configs/stm3210e-eval/dfu/setenv.sh
+#
+#   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$(basename $0)" = "setenv.sh" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi
+
+WD=`pwd`
+export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin"
+export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/px4io-v1/src/Makefile b/nuttx-configs/px4io-v1/src/Makefile
new file mode 100644
index 000000000..bb9539d16
--- /dev/null
+++ b/nuttx-configs/px4io-v1/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/stm3210e-eval/src/Makefile
+#
+#   Copyright (C) 2009-2010 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# 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 NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		= 
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	@$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	@touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
diff --git a/nuttx-configs/px4io-v1/src/README.txt b/nuttx-configs/px4io-v1/src/README.txt
new file mode 100644
index 000000000..d4eda82fd
--- /dev/null
+++ b/nuttx-configs/px4io-v1/src/README.txt
@@ -0,0 +1 @@
+This directory contains drivers unique to the STMicro STM3210E-EVAL development board.
diff --git a/nuttx-configs/px4io-v1/src/drv_i2c_device.c b/nuttx-configs/px4io-v1/src/drv_i2c_device.c
new file mode 100644
index 000000000..1f5931ae5
--- /dev/null
+++ b/nuttx-configs/px4io-v1/src/drv_i2c_device.c
@@ -0,0 +1,618 @@
+/****************************************************************************
+ *
+ *   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 A simple, polled I2C slave-mode driver.
+  *
+  * The master writes to and reads from a byte buffer, which the caller 
+  * can update inbetween calls to the FSM.
+  */
+
+#include <stdbool.h>
+
+#include "stm32_i2c.h"
+
+#include <string.h>
+
+/*
+ * I2C register definitions.
+ */
+#define I2C_BASE	STM32_I2C1_BASE
+
+#define REG(_reg)	(*(volatile uint32_t *)(I2C_BASE + _reg))
+
+#define rCR1		REG(STM32_I2C_CR1_OFFSET)
+#define rCR2		REG(STM32_I2C_CR2_OFFSET)
+#define rOAR1		REG(STM32_I2C_OAR1_OFFSET)
+#define rOAR2		REG(STM32_I2C_OAR2_OFFSET)
+#define rDR		REG(STM32_I2C_DR_OFFSET)
+#define rSR1		REG(STM32_I2C_SR1_OFFSET)
+#define rSR2		REG(STM32_I2C_SR2_OFFSET)
+#define rCCR		REG(STM32_I2C_CCR_OFFSET)
+#define rTRISE		REG(STM32_I2C_TRISE_OFFSET)
+
+/*
+ * "event" values (cr2 << 16 | cr1) as described in the ST DriverLib
+ */
+#define  I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED          ((uint32_t)0x00020002) /* BUSY and ADDR flags */
+#define  I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED       ((uint32_t)0x00060082) /* TRA, BUSY, TXE and ADDR flags */
+#define  I2C_EVENT_SLAVE_BYTE_RECEIVED                     ((uint32_t)0x00020040) /* BUSY and RXNE flags */
+#define  I2C_EVENT_SLAVE_STOP_DETECTED                     ((uint32_t)0x00000010) /* STOPF flag */
+#define  I2C_EVENT_SLAVE_BYTE_TRANSMITTED                  ((uint32_t)0x00060084) /* TRA, BUSY, TXE and BTF flags */
+#define  I2C_EVENT_SLAVE_BYTE_TRANSMITTING                 ((uint32_t)0x00060080) /* TRA, BUSY and TXE flags */
+#define  I2C_EVENT_SLAVE_ACK_FAILURE                       ((uint32_t)0x00000400) /* AF flag */
+
+/**
+ * States implemented by the I2C FSM.
+ */
+enum fsm_state {
+	BAD_PHASE,	 // must be zero, default exit on a bad state transition
+
+	WAIT_FOR_MASTER,
+
+	/* write from master */
+	WAIT_FOR_COMMAND,
+	RECEIVE_COMMAND,
+	RECEIVE_DATA,
+	HANDLE_COMMAND,
+
+	/* read from master */
+	WAIT_TO_SEND,
+	SEND_STATUS,
+	SEND_DATA,
+
+	NUM_STATES
+};
+
+/**
+ * Events recognised by the I2C FSM.
+ */
+enum fsm_event {
+	/* automatic transition */
+	AUTO,
+
+	/* write from master */
+	ADDRESSED_WRITE,
+	BYTE_RECEIVED,
+	STOP_RECEIVED,
+
+	/* read from master */
+	ADDRESSED_READ,
+	BYTE_SENDABLE,
+	ACK_FAILED,
+
+	NUM_EVENTS
+};
+
+/**
+ * Context for the I2C FSM
+ */
+static struct fsm_context {
+	enum fsm_state		state;
+
+	/* XXX want to eliminate these */
+	uint8_t			command;
+	uint8_t			status;
+
+	uint8_t			*data_ptr;
+	uint32_t		data_count;
+
+	size_t			buffer_size;
+	uint8_t			*buffer;
+} context;
+
+/**
+ * Structure defining one FSM state and its outgoing transitions.
+ */
+struct fsm_transition {
+		void (*handler)(void);
+		enum fsm_state next_state[NUM_EVENTS];
+};
+
+static bool	i2c_command_received;
+
+static void	fsm_event(enum fsm_event event);
+
+static void	go_bad(void);
+static void	go_wait_master(void);
+
+static void	go_wait_command(void);
+static void	go_receive_command(void);
+static void	go_receive_data(void);
+static void	go_handle_command(void);
+
+static void	go_wait_send(void);
+static void	go_send_status(void);
+static void	go_send_buffer(void);
+
+/**
+ * The FSM state graph.
+ */
+static const struct fsm_transition fsm[NUM_STATES] = {
+		[BAD_PHASE] = {
+				.handler = go_bad,
+				.next_state = {
+						[AUTO] = WAIT_FOR_MASTER,
+				},
+		},
+
+		[WAIT_FOR_MASTER] = {
+				.handler = go_wait_master,
+				.next_state = {
+						[ADDRESSED_WRITE] = WAIT_FOR_COMMAND,
+						[ADDRESSED_READ] = WAIT_TO_SEND,
+				},
+		},
+
+		/* write from master*/
+		[WAIT_FOR_COMMAND] = {
+				.handler = go_wait_command,
+				.next_state = {
+						[BYTE_RECEIVED] = RECEIVE_COMMAND,
+						[STOP_RECEIVED] = WAIT_FOR_MASTER,
+				},
+		},
+		[RECEIVE_COMMAND] = {
+				.handler = go_receive_command,
+				.next_state = {
+						[BYTE_RECEIVED] = RECEIVE_DATA,
+						[STOP_RECEIVED] = HANDLE_COMMAND,
+				},
+		},
+		[RECEIVE_DATA] = {
+				.handler = go_receive_data,
+				.next_state = {
+						[BYTE_RECEIVED] = RECEIVE_DATA,
+						[STOP_RECEIVED] = HANDLE_COMMAND,
+				},
+		},
+		[HANDLE_COMMAND] = {
+				.handler = go_handle_command,
+				.next_state = {
+						[AUTO] = WAIT_FOR_MASTER,
+				},
+		},
+
+		/* buffer send */
+		[WAIT_TO_SEND] = {
+			.handler = go_wait_send,
+			.next_state = {
+					[BYTE_SENDABLE] = SEND_STATUS,
+			},
+		},
+		[SEND_STATUS] = {
+				.handler = go_send_status,
+				.next_state = {
+						[BYTE_SENDABLE] = SEND_DATA,
+						[ACK_FAILED] = WAIT_FOR_MASTER,
+				},
+		},
+		[SEND_DATA] = {
+				.handler = go_send_buffer,
+				.next_state = {
+						[BYTE_SENDABLE] = SEND_DATA,
+						[ACK_FAILED] = WAIT_FOR_MASTER,
+				},
+		},
+};
+
+
+/* debug support */
+#if 1
+struct fsm_logentry {
+	char		kind;
+	uint32_t	code;
+};
+
+#define LOG_ENTRIES	32
+static struct fsm_logentry fsm_log[LOG_ENTRIES];
+int	fsm_logptr;
+#define LOG_NEXT(_x)	(((_x) + 1) % LOG_ENTRIES)
+#define LOGx(_kind, _code)		\
+		do {					\
+			fsm_log[fsm_logptr].kind = _kind; \
+			fsm_log[fsm_logptr].code = _code; \
+			fsm_logptr = LOG_NEXT(fsm_logptr); \
+			fsm_log[fsm_logptr].kind = 0; \
+		} while(0)
+
+#define LOG(_kind, _code) \
+		do {\
+			if (fsm_logptr < LOG_ENTRIES) { \
+				fsm_log[fsm_logptr].kind = _kind; \
+				fsm_log[fsm_logptr].code = _code; \
+				fsm_logptr++;\
+			}\
+		}while(0)
+
+#else
+#define LOG(_kind, _code)
+#endif
+
+
+static void i2c_setclock(uint32_t frequency);
+
+/**
+ * Initialise I2C
+ *
+ */
+void
+i2c_fsm_init(uint8_t *buffer, size_t buffer_size)
+{
+	/* save the buffer */
+	context.buffer = buffer;
+	context.buffer_size = buffer_size;
+
+	// initialise the FSM
+	context.status = 0;
+	context.command = 0;
+	context.state = BAD_PHASE;
+	fsm_event(AUTO);
+
+#if 0
+	// enable the i2c block clock and reset it
+	modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN);
+	modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST);
+	modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0);
+
+	// configure the i2c GPIOs
+	stm32_configgpio(GPIO_I2C1_SCL);
+	stm32_configgpio(GPIO_I2C1_SDA);
+
+	// set the peripheral clock to match the APB clock
+	rCR2 = STM32_PCLK1_FREQUENCY / 1000000;
+
+	// configure for 100kHz operation
+	i2c_setclock(100000);
+
+	// enable i2c
+	rCR1 = I2C_CR1_PE;
+#endif
+}
+
+/**
+ * Run the I2C FSM for some period.
+ *
+ * @return True if the buffer has been updated by a command.
+ */
+bool
+i2c_fsm(void)
+{
+	uint32_t	event;
+	int		idle_iterations = 0;
+
+	for (;;) {
+		// handle bus error states by discarding the current operation
+		if (rSR1 & I2C_SR1_BERR) {
+			context.state = WAIT_FOR_MASTER;
+			rSR1 = ~I2C_SR1_BERR;
+		}
+
+		// we do not anticipate over/underrun errors as clock-stretching is enabled
+
+		// fetch the most recent event
+		event = ((rSR2 << 16) | rSR1) & 0x00ffffff;
+
+		// generate FSM events based on I2C events
+		switch (event) {
+		case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED:
+			LOG('w', 0);
+			fsm_event(ADDRESSED_WRITE);
+			break;
+
+		case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED:
+			LOG('r', 0);
+			fsm_event(ADDRESSED_READ);
+			break;
+
+		case I2C_EVENT_SLAVE_BYTE_RECEIVED:
+			LOG('R', 0);
+			fsm_event(BYTE_RECEIVED);
+			break;
+
+		case I2C_EVENT_SLAVE_STOP_DETECTED:
+			LOG('s', 0);
+			fsm_event(STOP_RECEIVED);
+			break;
+
+		case I2C_EVENT_SLAVE_BYTE_TRANSMITTING:
+		//case I2C_EVENT_SLAVE_BYTE_TRANSMITTED:
+			LOG('T', 0);
+			fsm_event(BYTE_SENDABLE);
+			break;
+
+		case I2C_EVENT_SLAVE_ACK_FAILURE:
+			LOG('a', 0);
+			fsm_event(ACK_FAILED);
+			break;
+
+		default:
+			idle_iterations++;
+//			if ((event) && (event != 0x00020000))
+//				LOG('e', event);
+			break;
+		}
+
+		/* if we have just received something, drop out and let the caller handle it */
+		if (i2c_command_received) {
+			i2c_command_received = false;
+			return true;
+		}
+
+		/* if we have done nothing recently, drop out and let the caller have a slice */
+		if (idle_iterations > 1000)
+			return false;
+	}
+}
+
+/**
+ * Update the FSM with an event
+ *
+ * @param event	New event.
+ */
+static void
+fsm_event(enum fsm_event event)
+{
+	// move to the next state
+	context.state = fsm[context.state].next_state[event];
+
+	LOG('f', context.state);
+
+	// call the state entry handler
+	if (fsm[context.state].handler) {
+		fsm[context.state].handler();
+	}
+}
+
+static void
+go_bad()
+{
+	LOG('B', 0);
+	fsm_event(AUTO);
+}
+
+/**
+ * Wait for the master to address us.
+ *
+ */
+static void
+go_wait_master()
+{
+	// We currently don't have a command byte.
+	//
+	context.command = '\0';
+
+	// The data pointer starts pointing to the start of the data buffer.
+	//
+	context.data_ptr = context.buffer;
+
+	// The data count is either:
+	// - the size of the data buffer
+	// - some value less than or equal the size of the data buffer during a write or a read
+	//
+	context.data_count = context.buffer_size;
+
+	// (re)enable the peripheral, clear the stop event flag in
+	// case we just finished receiving data
+	rCR1 |= I2C_CR1_PE;
+
+	// clear the ACK failed flag in case we just finished sending data
+	rSR1 = ~I2C_SR1_AF;
+}
+
+/**
+ * Prepare to receive a command byte.
+ *
+ */
+static void
+go_wait_command()
+{
+	// NOP
+}
+
+/**
+ * Command byte has been received, save it and prepare to handle the data.
+ *
+ */
+static void
+go_receive_command()
+{
+
+	// fetch the command byte
+	context.command = (uint8_t)rDR;
+	LOG('c', context.command);
+
+}
+
+/**
+ * Receive a data byte.
+ *
+ */
+static void
+go_receive_data()
+{
+	uint8_t	d;
+
+	// fetch the byte
+	d = (uint8_t)rDR;
+	LOG('d', d);
+
+	// if we have somewhere to put it, do so
+	if (context.data_count) {
+		*context.data_ptr++ = d;
+		context.data_count--;
+	}
+}
+
+/**
+ * Handle a command once the host is done sending it to us.
+ *
+ */
+static void
+go_handle_command()
+{
+	// presume we are happy with the command
+	context.status = 0;
+
+	// make a note that the buffer contains a fresh command
+	i2c_command_received = true;
+
+	// kick along to the next state
+	fsm_event(AUTO);
+}
+
+/**
+ * Wait to be able to send the status byte.
+ *
+ */
+static void
+go_wait_send()
+{
+	// NOP
+}
+
+/**
+ * Send the status byte.
+ *
+ */
+static void
+go_send_status()
+{
+	rDR = context.status;
+	LOG('?', context.status);
+}
+
+/**
+ * Send a data or pad byte.
+ *
+ */
+static void
+go_send_buffer()
+{
+	if (context.data_count) {
+		LOG('D', *context.data_ptr);
+		rDR = *(context.data_ptr++);
+		context.data_count--;
+	} else {
+		LOG('-', 0);
+		rDR = 0xff;
+	}
+}
+
+/* cribbed directly from the NuttX master driver */
+static void 
+i2c_setclock(uint32_t frequency)
+{
+	uint16_t cr1;
+	uint16_t ccr;
+	uint16_t trise;
+	uint16_t freqmhz;
+	uint16_t speed;
+
+	/* Disable the selected I2C peripheral to configure TRISE */
+
+	cr1 = rCR1;
+	rCR1 &= ~I2C_CR1_PE;
+
+	/* Update timing and control registers */
+
+	freqmhz = (uint16_t)(STM32_PCLK1_FREQUENCY / 1000000);
+	ccr = 0;
+
+	/* Configure speed in standard mode */
+
+	if (frequency <= 100000) {
+		/* Standard mode speed calculation */
+
+		speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency << 1));
+
+		/* The CCR fault must be >= 4 */
+
+		if (speed < 4) {
+			/* Set the minimum allowed value */
+
+			speed = 4;  
+		}
+		ccr |= speed;
+
+		/* Set Maximum Rise Time for standard mode */
+
+		trise = freqmhz + 1; 
+				
+	/* Configure speed in fast mode */
+	} else { /* (frequency <= 400000) */
+		/* Fast mode speed calculation with Tlow/Thigh = 16/9 */
+
+#ifdef CONFIG_I2C_DUTY16_9
+	      speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25));
+
+		/* Set DUTY and fast speed bits */
+
+		ccr |= (I2C_CCR_DUTY|I2C_CCR_FS);
+#else
+		/* Fast mode speed calculation with Tlow/Thigh = 2 */
+
+		speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 3));
+
+		/* Set fast speed bit */
+
+		ccr |= I2C_CCR_FS;
+#endif
+
+		/* Verify that the CCR speed value is nonzero */
+
+		if (speed < 1) {
+	        	/* Set the minimum allowed value */
+
+	        	speed = 1;  
+	        }
+		ccr |= speed;
+
+		/* Set Maximum Rise Time for fast mode */
+
+		trise = (uint16_t)(((freqmhz * 300) / 1000) + 1);  
+	}
+
+	/* Write the new values of the CCR and TRISE registers */
+
+	rCCR = ccr;
+	rTRISE = trise;
+
+	/* Bit 14 of OAR1 must be configured and kept at 1 */
+
+	rOAR1 = I2C_OAR1_ONE);
+
+	/* Re-enable the peripheral (or not) */
+
+	rCR1 = cr1;
+}
diff --git a/nuttx-configs/px4io-v1/src/empty.c b/nuttx-configs/px4io-v1/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/px4io-v1/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so 
+ * we have this empty source file to keep it company.
+ */
diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h
index b93ad4220..4b9c90638 100755
--- a/nuttx-configs/px4io-v2/include/board.h
+++ b/nuttx-configs/px4io-v2/include/board.h
@@ -112,34 +112,6 @@
 #undef GPIO_USART3_RTS
 #define GPIO_USART3_RTS	0xffffffff
 
-/* 
- * High-resolution timer
- */
-#define HRT_TIMER		1	/* use timer1 for the HRT */
-#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
-
-/*
- * PPM
- *
- * PPM input is handled by the HRT timer.
- *
- * Pin is PA8, timer 1, channel 1
- */
-#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
-#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
-
-/* LED definitions ******************************************************************/
-/* PX4 has two LEDs that we will encode as: */
-
-#define LED_STARTED       0  /* LED? */
-#define LED_HEAPALLOCATE  1  /* LED? */
-#define LED_IRQSENABLED   2  /* LED? + LED? */
-#define LED_STACKCREATED  3  /* LED? */
-#define LED_INIRQ         4  /* LED? + LED? */
-#define LED_SIGNAL        5  /* LED? + LED? */
-#define LED_ASSERTION     6  /* LED? + LED? + LED? */
-#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
-
 /************************************************************************************
  * Public Data
  ************************************************************************************/
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 8d2eb056e..b4c5fa06e 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -92,12 +92,6 @@
 
 #include <nuttx/config.h>
 
-__BEGIN_DECLS
-#include <arch/board/board.h>
-__END_DECLS
-#include <systemlib/systemlib.h>
-#include <drivers/device/i2c.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <string.h>
@@ -107,15 +101,19 @@ __END_DECLS
 #include <unistd.h>
 #include <stdio.h>
 #include <ctype.h>
-
-#include <drivers/drv_blinkm.h>
+#include <poll.h>
 
 #include <nuttx/wqueue.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <board_config.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_blinkm.h>
 
-#include <poll.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_gps_position.h>
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 4409a8a9c..cfb625670 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -59,7 +59,7 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
new file mode 100644
index 000000000..9d7c81f85
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -0,0 +1,209 @@
+/****************************************************************************
+ *
+ *   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 board_config.h
+ *
+ * PX4FMU internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+ 
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS2"
+
+//#ifdef CONFIG_STM32_SPI2
+//#  error "SPI2 is not supported on this board"
+//#endif
+
+#if defined(CONFIG_STM32_CAN1)
+#  warning "CAN1 is not supported on this board"
+#endif
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+
+/* External interrupts */
+#define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+
+/*
+ * Use these in place of the spi_dev_e enumeration to
+ * select a specific SPI device on SPI1
+ */
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_ACCEL	2
+#define PX4_SPIDEV_MPU		3
+
+/*
+ * Optional devices on IO's external port
+ */
+#define PX4_SPIDEV_ACCEL_MAG 2
+
+/*
+ * I2C busses
+ */
+#define PX4_I2C_BUS_ESC		1
+#define PX4_I2C_BUS_ONBOARD	2
+#define PX4_I2C_BUS_EXPANSION	3
+
+/*
+ * Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+#define PX4_I2C_OBDEV_MS5611	0x76
+#define PX4_I2C_OBDEV_EEPROM	NOTDEFINED
+
+#define PX4_I2C_OBDEV_PX4IO_BL	0x18
+#define PX4_I2C_OBDEV_PX4IO	0x1a
+
+/* User GPIOs
+ *
+ * GPIO0-1 are the buffered high-power GPIOs.
+ * GPIO2-5 are the USART2 pins.
+ * GPIO6-7 are the CAN2 pins.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+/*
+ * Tone alarm output
+ */
+#define TONE_ALARM_TIMER	3	/* timer 3 */
+#define TONE_ALARM_CHANNEL	3	/* channel 3 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
+
+/*
+ * PWM
+ *
+ * Four PWM outputs can be configured on pins otherwise shared with
+ * USART2; two can take the flow control pins if they are not being used.
+ *
+ * Pins:
+ *
+ * CTS - PA0 - TIM2CH1
+ * RTS - PA1 - TIM2CH2
+ * TX  - PA2 - TIM2CH3
+ * RX  - PA3 - TIM2CH4
+ *
+ */
+#define GPIO_TIM2_CH1OUT	GPIO_TIM2_CH1OUT_1
+#define GPIO_TIM2_CH2OUT	GPIO_TIM2_CH2OUT_1
+#define GPIO_TIM2_CH3OUT	GPIO_TIM2_CH3OUT_1
+#define GPIO_TIM2_CH4OUT	GPIO_TIM2_CH4OUT_1
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+#define HRT_PPM_CHANNEL		3	/* use capture/compare channel 3 */
+#define GPIO_PPM_IN		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk
new file mode 100644
index 000000000..66b776917
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v1/module.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific startup code for the PX4FMU
+#
+
+SRCS		 = px4fmu_can.c \
+		   px4fmu_init.c \
+		   px4fmu_pwm_servo.c \
+		   px4fmu_spi.c \
+		   px4fmu_usb.c \
+		   px4fmu_led.c
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
new file mode 100644
index 000000000..1e1f10040
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+#  define candbg    dbg
+#  define canvdbg   vdbg
+#  define canlldbg  lldbg
+#  define canllvdbg llvdbg
+#else
+#  define candbg(x...)
+#  define canvdbg(x...)
+#  define canlldbg(x...)
+#  define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			candbg("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			candbg("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
new file mode 100644
index 000000000..964f5069c
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu_init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+
+#include "stm32.h"
+#include "board_config.h"
+#include "stm32_uart.h"
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs (empty call to NuttX' ledinit) */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+static struct spi_dev_s *spi3;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+	int result;
+
+	/* configure always-on ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN10);
+	stm32_configgpio(GPIO_ADC1_IN11);
+	/* IN12 and IN13 further below */
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+	led_off(LED_BLUE);
+
+
+	/* Configure SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\r\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
+	up_udelay(20);
+
+	message("[boot] Successfully initialized SPI port 1\r\n");
+
+	/*
+	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
+	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
+	 */
+	spi2 = up_spiinitialize(2);
+
+	if (!spi2) {
+		message("[boot] Enabling IN12/13 instead of SPI2\n");
+		/* no SPI2, use pins for ADC */
+		stm32_configgpio(GPIO_ADC1_IN12);
+		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
+	} else {
+		/* Default SPI2 to 1MHz and de-assert the known chip selects. */
+		SPI_SETFREQUENCY(spi2, 10000000);
+		SPI_SETBITS(spi2, 8);
+		SPI_SETMODE(spi2, SPIDEV_MODE3);
+		SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false);
+		SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
+
+		message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
+	}
+
+	/* Get the SPI port for the microSD slot */
+
+	message("[boot] Initializing SPI port 3\n");
+	spi3 = up_spiinitialize(3);
+
+	if (!spi3) {
+		message("[boot] FAILED to initialize SPI port 3\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	message("[boot] Successfully initialized SPI port 3\n");
+
+	/* Now bind the SPI interface to the MMCSD driver */
+	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
+
+	if (result != OK) {
+		message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
+
+	return OK;
+}
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
new file mode 100644
index 000000000..aa83ec130
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+	/* Configure LED1-2 GPIOs for output */
+
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 0)
+	{
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED1, false);
+	}
+	if (led == 1)
+	{
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED2, false);
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 0)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED1, true);
+	}
+	if (led == 1)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED2, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
new file mode 100644
index 000000000..848e21d79
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
new file mode 100644
index 000000000..17e6862f7
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c
@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include "up_arch.h"
+#include "chip.h"
+#include "stm32.h"
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL);
+	stm32_configgpio(GPIO_SPI_CS_MPU);
+	stm32_configgpio(GPIO_SPI_CS_SDCARD);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+		break;
+
+	case PX4_SPIDEV_ACCEL:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		break;
+
+	case PX4_SPIDEV_MPU:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
+		break;
+
+	default:
+		break;
+
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+		break;
+
+	default:
+		break;
+
+	}
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
+}
+
+__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* this is actually bogus, but PX4 has no way to sense the presence of an SD card */
+	return SPI_STATUS_PRESENT;
+}
+
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
new file mode 100644
index 000000000..0fc8569aa
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include "up_arch.h"
+#include "stm32.h"
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
new file mode 100644
index 000000000..ec8dde499
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -0,0 +1,192 @@
+/****************************************************************************
+ *
+ *   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 board_config.h
+ *
+ * PX4FMU internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+ 
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS4"
+#define PX4IO_SERIAL_TX_GPIO	GPIO_USART6_TX
+#define PX4IO_SERIAL_RX_GPIO	GPIO_USART6_RX
+#define PX4IO_SERIAL_BASE	STM32_USART6_BASE	/* hardwired on the board */
+#define PX4IO_SERIAL_VECTOR	STM32_IRQ_USART6
+#define PX4IO_SERIAL_TX_DMAMAP	DMAMAP_USART6_TX_2
+#define PX4IO_SERIAL_RX_DMAMAP	DMAMAP_USART6_RX_2
+#define PX4IO_SERIAL_CLOCK	STM32_PCLK2_FREQUENCY
+#define PX4IO_SERIAL_BITRATE	1500000			/* 1.5Mbps -> max rate for IO */
+
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+
+/* External interrupts */
+#define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_EXTI_MAG_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_EXTI_ACCEL_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4)
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+
+/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_ACCEL_MAG	2
+#define PX4_SPIDEV_BARO		3
+
+/* I2C busses */
+#define PX4_I2C_BUS_EXPANSION	1
+#define PX4_I2C_BUS_LED		2
+
+/* Devices on the onboard bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_LED	0x55
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+
+/* User GPIOs
+ *
+ * GPIO0-5 are the PWM servo outputs.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_VDD_5V_PERIPH_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define GPIO_VDD_BRICK_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_VDD_SERVO_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7)
+#define GPIO_VDD_3V3_SENSORS_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_VDD_5V_HIPOWER_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)
+#define GPIO_VDD_5V_PERIPH_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
+
+/* Tone alarm output */
+#define TONE_ALARM_TIMER	2	/* timer 2 */
+#define TONE_ALARM_CHANNEL	1	/* channel 1 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
+
+/* PWM
+ *
+ * Six PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PE14 : TIM1_CH4
+ * CH2 : PE13 : TIM1_CH3
+ * CH3 : PE11 : TIM1_CH2
+ * CH4 : PE9  : TIM1_CH1
+ * CH5 : PD13 : TIM4_CH2
+ * CH6 : PD14 : TIM4_CH3
+ */
+#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_2
+#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_2
+#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_2
+#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_2
+#define GPIO_TIM4_CH2OUT	GPIO_TIM4_CH2OUT_2
+#define GPIO_TIM4_CH3OUT	GPIO_TIM4_CH3OUT_2
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER		8	/* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk
new file mode 100644
index 000000000..99d37eeca
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/module.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific startup code for the PX4FMUv2
+#
+
+SRCS		 = px4fmu_can.c \
+		   px4fmu2_init.c \
+		   px4fmu_pwm_servo.c \
+		   px4fmu_spi.c \
+		   px4fmu_usb.c \
+		   px4fmu2_led.c
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
new file mode 100644
index 000000000..135767b26
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c
@@ -0,0 +1,262 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+static struct sdio_dev_s *sdio;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
+	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
+	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
+	stm32_configgpio(GPIO_ADC1_IN10);	/* unrouted */
+	stm32_configgpio(GPIO_ADC1_IN11);	/* unrouted */
+	stm32_configgpio(GPIO_ADC1_IN12);	/* unrouted */
+	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
+	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
+	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
+
+	/* configure power supply control/sense pins */
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	stm32_configgpio(GPIO_VDD_BRICK_VALID);
+	stm32_configgpio(GPIO_VDD_SERVO_VALID);
+	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+
+	/* Configure SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+	up_udelay(20);
+
+	message("[boot] Successfully initialized SPI port 1\n");
+
+	/* Get the SPI port for the FRAM */
+
+	spi2 = up_spiinitialize(2);
+
+	if (!spi2) {
+		message("[boot] FAILED to initialize SPI port 2\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+
+	/* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi2, 375000000);
+	SPI_SETBITS(spi2, 8);
+	SPI_SETMODE(spi2, SPIDEV_MODE3);
+	SPI_SELECT(spi2, SPIDEV_FLASH, false);
+
+	message("[boot] Successfully initialized SPI port 2\n");
+
+	#ifdef CONFIG_MMCSD
+	/* First, get an instance of the SDIO interface */
+
+	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
+	if (!sdio) {
+		message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
+			CONFIG_NSH_MMCSDSLOTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SDIO interface to the MMC/SD driver */
+	int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
+	if (ret != OK) {
+		message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		return ret;
+	}
+
+	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
+	sdio_mediachange(sdio, true);
+
+	message("[boot] Initialized SDIO\n");
+	#endif
+
+	return OK;
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
new file mode 100644
index 000000000..5ded4294e
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4fmu2_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+	/* Configure LED1 GPIO for output */
+
+	stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+	if (led == 1)
+	{
+		/* Pull down to switch on */
+		stm32_gpiowrite(GPIO_LED1, false);
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	if (led == 1)
+	{
+		/* Pull up to switch off */
+		stm32_gpiowrite(GPIO_LED1, true);
+	}
+}
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
new file mode 100644
index 000000000..f66c7cd79
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_can.c
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+#  define candbg    dbg
+#  define canvdbg   vdbg
+#  define canlldbg  lldbg
+#  define canllvdbg llvdbg
+#else
+#  define candbg(x...)
+#  define canvdbg(x...)
+#  define canlldbg(x...)
+#  define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			candbg("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			candbg("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif
\ No newline at end of file
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
new file mode 100644
index 000000000..600dfef41
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM1_BASE,
+		.clock_register = STM32_RCC_APB2ENR,
+		.clock_bit = RCC_APB2ENR_TIM1EN,
+		.clock_freq = STM32_APB2_TIM1_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
new file mode 100644
index 000000000..09838d02f
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI_CS_FRAM);
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+#endif
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_ACCEL_MAG:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* FRAM is always present */
+	return SPI_STATUS_PRESENT;
+}
+#endif
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
new file mode 100644
index 000000000..f329e06ff
--- /dev/null
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   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 px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <up_arch.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+
diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk
deleted file mode 100644
index 66b776917..000000000
--- a/src/drivers/boards/px4fmu/module.mk
+++ /dev/null
@@ -1,10 +0,0 @@
-#
-# Board-specific startup code for the PX4FMU
-#
-
-SRCS		 = px4fmu_can.c \
-		   px4fmu_init.c \
-		   px4fmu_pwm_servo.c \
-		   px4fmu_spi.c \
-		   px4fmu_usb.c \
-		   px4fmu_led.c
diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu/px4fmu_can.c
deleted file mode 100644
index 187689ff9..000000000
--- a/src/drivers/boards/px4fmu/px4fmu_can.c
+++ /dev/null
@@ -1,144 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_can.c
- *
- * Board-specific CAN functions.
- */
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <errno.h>
-#include <debug.h>
-
-#include <nuttx/can.h>
-#include <arch/board/board.h>
-
-#include "chip.h"
-#include "up_arch.h"
-
-#include "stm32.h"
-#include "stm32_can.h"
-#include "px4fmu_internal.h"
-
-#ifdef CONFIG_CAN
-
-/************************************************************************************
- * Pre-processor Definitions
- ************************************************************************************/
-/* Configuration ********************************************************************/
-
-#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
-#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
-#  undef CONFIG_STM32_CAN2
-#endif
-
-#ifdef CONFIG_STM32_CAN1
-#  define CAN_PORT 1
-#else
-#  define CAN_PORT 2
-#endif
-
-/* Debug ***************************************************************************/
-/* Non-standard debug that may be enabled just for testing CAN */
-
-#ifdef CONFIG_DEBUG_CAN
-#  define candbg    dbg
-#  define canvdbg   vdbg
-#  define canlldbg  lldbg
-#  define canllvdbg llvdbg
-#else
-#  define candbg(x...)
-#  define canvdbg(x...)
-#  define canlldbg(x...)
-#  define canllvdbg(x...)
-#endif
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: can_devinit
- *
- * Description:
- *   All STM32 architectures must provide the following interface to work with
- *   examples/can.
- *
- ************************************************************************************/
-
-int can_devinit(void)
-{
-	static bool initialized = false;
-	struct can_dev_s *can;
-	int ret;
-
-	/* Check if we have already initialized */
-
-	if (!initialized) {
-		/* Call stm32_caninitialize() to get an instance of the CAN interface */
-
-		can = stm32_caninitialize(CAN_PORT);
-
-		if (can == NULL) {
-			candbg("ERROR:  Failed to get CAN interface\n");
-			return -ENODEV;
-		}
-
-		/* Register the CAN driver at "/dev/can0" */
-
-		ret = can_register("/dev/can0", can);
-
-		if (ret < 0) {
-			candbg("ERROR: can_register failed: %d\n", ret);
-			return ret;
-		}
-
-		/* Now we are initialized */
-
-		initialized = true;
-	}
-
-	return OK;
-}
-
-#endif
diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c
deleted file mode 100644
index 36af2298c..000000000
--- a/src/drivers/boards/px4fmu/px4fmu_init.c
+++ /dev/null
@@ -1,268 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file px4fmu_init.c
- *
- * PX4FMU-specific early startup code.  This file implements the
- * nsh_archinitialize() function that is called early by nsh during startup.
- *
- * Code here is run before the rcS script is invoked; it should start required
- * subsystems and perform board-specific initialisation.
- */
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdbool.h>
-#include <stdio.h>
-#include <debug.h>
-#include <errno.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/spi.h>
-#include <nuttx/i2c.h>
-#include <nuttx/mmcsd.h>
-#include <nuttx/analog/adc.h>
-
-#include "stm32.h"
-#include "px4fmu_internal.h"
-#include "stm32_uart.h"
-
-#include <arch/board/board.h>
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_led.h>
-
-#include <systemlib/cpuload.h>
-
-/****************************************************************************
- * Pre-Processor Definitions
- ****************************************************************************/
-
-/* Configuration ************************************************************/
-
-/* Debug ********************************************************************/
-
-#ifdef CONFIG_CPP_HAVE_VARARGS
-#  ifdef CONFIG_DEBUG
-#    define message(...) lowsyslog(__VA_ARGS__)
-#  else
-#    define message(...) printf(__VA_ARGS__)
-#  endif
-#else
-#  ifdef CONFIG_DEBUG
-#    define message lowsyslog
-#  else
-#    define message printf
-#  endif
-#endif
-
-/*
- * Ideally we'd be able to get these from up_internal.h,
- * but since we want to be able to disable the NuttX use
- * of leds for system indication at will and there is no
- * separate switch, we need to build independent of the
- * CONFIG_ARCH_LEDS configuration switch.
- */
-__BEGIN_DECLS
-extern void led_init();
-extern void led_on(int led);
-extern void led_off(int led);
-__END_DECLS
-
-/****************************************************************************
- * Protected Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/************************************************************************************
- * Name: stm32_boardinitialize
- *
- * Description:
- *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
- *   and mapped but before any devices have been initialized.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_boardinitialize(void)
-{
-	/* configure SPI interfaces */
-	stm32_spiinitialize();
-
-	/* configure LEDs (empty call to NuttX' ledinit) */
-	up_ledinit();
-}
-
-/****************************************************************************
- * Name: nsh_archinitialize
- *
- * Description:
- *   Perform architecture specific initialization
- *
- ****************************************************************************/
-
-static struct spi_dev_s *spi1;
-static struct spi_dev_s *spi2;
-static struct spi_dev_s *spi3;
-
-#include <math.h>
-
-#ifdef __cplusplus
-__EXPORT int matherr(struct __exception *e)
-{
-	return 1;
-}
-#else
-__EXPORT int matherr(struct exception *e)
-{
-	return 1;
-}
-#endif
-
-__EXPORT int nsh_archinitialize(void)
-{
-	int result;
-
-	/* configure always-on ADC pins */
-	stm32_configgpio(GPIO_ADC1_IN10);
-	stm32_configgpio(GPIO_ADC1_IN11);
-	/* IN12 and IN13 further below */
-
-	/* configure the high-resolution time/callout interface */
-	hrt_init();
-
-	/* configure CPU load estimation */
-#ifdef CONFIG_SCHED_INSTRUMENTATION
-	cpuload_initialize_once();
-#endif
-
-	/* set up the serial DMA polling */
-	static struct hrt_call serial_dma_call;
-	struct timespec ts;
-
-	/*
-	 * Poll at 1ms intervals for received bytes that have not triggered
-	 * a DMA event.
-	 */
-	ts.tv_sec = 0;
-	ts.tv_nsec = 1000000;
-
-	hrt_call_every(&serial_dma_call,
-		       ts_to_abstime(&ts),
-		       ts_to_abstime(&ts),
-		       (hrt_callout)stm32_serial_dma_poll,
-		       NULL);
-
-	/* initial LED state */
-	drv_led_start();
-	led_off(LED_AMBER);
-	led_off(LED_BLUE);
-
-
-	/* Configure SPI-based devices */
-
-	spi1 = up_spiinitialize(1);
-
-	if (!spi1) {
-		message("[boot] FAILED to initialize SPI port 1\r\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
-	SPI_SETFREQUENCY(spi1, 10000000);
-	SPI_SETBITS(spi1, 8);
-	SPI_SETMODE(spi1, SPIDEV_MODE3);
-	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
-	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
-	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
-	up_udelay(20);
-
-	message("[boot] Successfully initialized SPI port 1\r\n");
-
-	/*
-	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
-	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
-	 */
-	spi2 = up_spiinitialize(2);
-
-	if (!spi2) {
-		message("[boot] Enabling IN12/13 instead of SPI2\n");
-		/* no SPI2, use pins for ADC */
-		stm32_configgpio(GPIO_ADC1_IN12);
-		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
-	} else {
-		/* Default SPI2 to 1MHz and de-assert the known chip selects. */
-		SPI_SETFREQUENCY(spi2, 10000000);
-		SPI_SETBITS(spi2, 8);
-		SPI_SETMODE(spi2, SPIDEV_MODE3);
-		SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false);
-		SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
-
-		message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
-	}
-
-	/* Get the SPI port for the microSD slot */
-
-	message("[boot] Initializing SPI port 3\n");
-	spi3 = up_spiinitialize(3);
-
-	if (!spi3) {
-		message("[boot] FAILED to initialize SPI port 3\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	message("[boot] Successfully initialized SPI port 3\n");
-
-	/* Now bind the SPI interface to the MMCSD driver */
-	result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
-
-	if (result != OK) {
-		message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
-
-	return OK;
-}
diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h
deleted file mode 100644
index 56173abf6..000000000
--- a/src/drivers/boards/px4fmu/px4fmu_internal.h
+++ /dev/null
@@ -1,165 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_internal.h
- *
- * PX4FMU internal definitions
- */
-
-#pragma once
-
-/****************************************************************************************************
- * Included Files
- ****************************************************************************************************/
-
-#include <nuttx/config.h>
-#include <nuttx/compiler.h>
-#include <stdint.h>
-
-__BEGIN_DECLS
-
-/* these headers are not C++ safe */
-#include <stm32.h>
-
- 
-/****************************************************************************************************
- * Definitions
- ****************************************************************************************************/
-/* Configuration ************************************************************************************/
-
-/* PX4IO connection configuration */
-#define PX4IO_SERIAL_DEVICE	"/dev/ttyS2"
-
-//#ifdef CONFIG_STM32_SPI2
-//#  error "SPI2 is not supported on this board"
-//#endif
-
-#if defined(CONFIG_STM32_CAN1)
-#  warning "CAN1 is not supported on this board"
-#endif
-
-/* PX4FMU GPIOs ***********************************************************************************/
-/* LEDs */
-
-#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
-
-/* External interrupts */
-#define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
-// XXX MPU6000 DRDY?
-
-/* SPI chip selects */
-
-#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
-#define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
-#define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
-
-/* User GPIOs
- *
- * GPIO0-1 are the buffered high-power GPIOs.
- * GPIO2-5 are the USART2 pins.
- * GPIO6-7 are the CAN2 pins.
- */
-#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
-#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
-#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
-#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
-#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13)
-#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2)
-#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
-#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
-#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
-#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
-#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
-#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
-#define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
-
-/* USB OTG FS
- *
- * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
- */
-#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
-
-/* PWM
- *
- * The PX4FMU has five PWM outputs, of which only the output on
- * pin PC8 is fixed assigned to this function. The other four possible
- * pwm sources are the TX, RX, RTS and CTS pins of USART2
- *
- * Alternate function mapping:
- * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
- */
-
-#ifdef CONFIG_PWM
-#  if defined(CONFIG_STM32_TIM3_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#	 define BUZZER_PWMTIMER 3
-#  elif defined(CONFIG_STM32_TIM8_PWM)
-#    define BUZZER_PWMCHANNEL 3
-#    define BUZZER_PWMTIMER 8
-#  endif
-#endif
-
-/****************************************************************************************************
- * Public Types
- ****************************************************************************************************/
-
-/****************************************************************************************************
- * Public data
- ****************************************************************************************************/
-
-#ifndef __ASSEMBLY__
-
-/****************************************************************************************************
- * Public Functions
- ****************************************************************************************************/
-
-/****************************************************************************************************
- * Name: stm32_spiinitialize
- *
- * Description:
- *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
- *
- ****************************************************************************************************/
-
-extern void stm32_spiinitialize(void);
-
-#endif /* __ASSEMBLY__ */
-
-__END_DECLS
diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c
deleted file mode 100644
index 31b25984e..000000000
--- a/src/drivers/boards/px4fmu/px4fmu_led.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_led.c
- *
- * PX4FMU LED backend.
- */
-
-#include <nuttx/config.h>
-
-#include <stdbool.h>
-
-#include "stm32.h"
-#include "px4fmu_internal.h"
-
-#include <arch/board/board.h>
-
-/*
- * Ideally we'd be able to get these from up_internal.h,
- * but since we want to be able to disable the NuttX use
- * of leds for system indication at will and there is no
- * separate switch, we need to build independent of the
- * CONFIG_ARCH_LEDS configuration switch.
- */
-__BEGIN_DECLS
-extern void led_init();
-extern void led_on(int led);
-extern void led_off(int led);
-__END_DECLS
-
-__EXPORT void led_init()
-{
-	/* Configure LED1-2 GPIOs for output */
-
-	stm32_configgpio(GPIO_LED1);
-	stm32_configgpio(GPIO_LED2);
-}
-
-__EXPORT void led_on(int led)
-{
-	if (led == 0)
-	{
-		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED1, false);
-	}
-	if (led == 1)
-	{
-		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED2, false);
-	}
-}
-
-__EXPORT void led_off(int led)
-{
-	if (led == 0)
-	{
-		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED1, true);
-	}
-	if (led == 1)
-	{
-		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED2, true);
-	}
-}
diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
deleted file mode 100644
index d85131dd8..000000000
--- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_pwm_servo.c
- *
- * Configuration data for the stm32 pwm_servo driver.
- *
- * Note that these arrays must always be fully-sized.
- */
-
-#include <stdint.h>
-
-#include <drivers/stm32/drv_pwm_servo.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_pwm_output.h>
-
-#include <stm32.h>
-#include <stm32_gpio.h>
-#include <stm32_tim.h>
-
-__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
-	{
-		.base = STM32_TIM2_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM2EN,
-		.clock_freq = STM32_APB1_TIM2_CLKIN
-	}
-};
-
-__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
-	{
-		.gpio = GPIO_TIM2_CH1OUT,
-		.timer_index = 0,
-		.timer_channel = 1,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH2OUT,
-		.timer_index = 0,
-		.timer_channel = 2,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH3OUT,
-		.timer_index = 0,
-		.timer_channel = 3,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH4OUT,
-		.timer_index = 0,
-		.timer_channel = 4,
-		.default_value = 1000,
-	}
-};
diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c
deleted file mode 100644
index e05ddecf3..000000000
--- a/src/drivers/boards/px4fmu/px4fmu_spi.c
+++ /dev/null
@@ -1,154 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_spi.c
- *
- * Board-specific SPI functions.
- */
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <nuttx/spi.h>
-#include <arch/board/board.h>
-
-#include "up_arch.h"
-#include "chip.h"
-#include "stm32.h"
-#include "px4fmu_internal.h"
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: stm32_spiinitialize
- *
- * Description:
- *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
- *
- ************************************************************************************/
-
-__EXPORT void weak_function stm32_spiinitialize(void)
-{
-	stm32_configgpio(GPIO_SPI_CS_GYRO);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL);
-	stm32_configgpio(GPIO_SPI_CS_MPU);
-	stm32_configgpio(GPIO_SPI_CS_SDCARD);
-
-	/* De-activate all peripherals,
-	 * required for some peripheral
-	 * state machines
-	 */
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1);
-}
-
-__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
-{
-	/* SPI select is active low, so write !selected to select the device */
-
-	switch (devid) {
-	case PX4_SPIDEV_GYRO:
-		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
-		break;
-
-	case PX4_SPIDEV_ACCEL:
-		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		break;
-
-	case PX4_SPIDEV_MPU:
-		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
-		break;
-
-	default:
-		break;
-
-	}
-}
-
-__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
-{
-	return SPI_STATUS_PRESENT;
-}
-
-__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
-{
-	/* SPI select is active low, so write !selected to select the device */
-
-	switch (devid) {
-		break;
-
-	default:
-		break;
-
-	}
-}
-
-__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
-{
-	return SPI_STATUS_PRESENT;
-}
-
-
-__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
-{
-	/* there can only be one device on this bus, so always select it */
-	stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
-}
-
-__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
-{
-	/* this is actually bogus, but PX4 has no way to sense the presence of an SD card */
-	return SPI_STATUS_PRESENT;
-}
-
diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c
deleted file mode 100644
index 0be981c1e..000000000
--- a/src/drivers/boards/px4fmu/px4fmu_usb.c
+++ /dev/null
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_usb.c
- *
- * Board-specific USB functions.
- */
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <nuttx/usb/usbdev.h>
-#include <nuttx/usb/usbdev_trace.h>
-
-#include "up_arch.h"
-#include "stm32.h"
-#include "px4fmu_internal.h"
-
-/************************************************************************************
- * Definitions
- ************************************************************************************/
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: stm32_usbinitialize
- *
- * Description:
- *   Called to setup USB-related GPIO pins for the PX4FMU board.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_usbinitialize(void)
-{
-	/* The OTG FS has an internal soft pull-up */
-
-	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
-
-#ifdef CONFIG_STM32_OTGFS
-	stm32_configgpio(GPIO_OTGFS_VBUS);
-	/* XXX We only support device mode
-	stm32_configgpio(GPIO_OTGFS_PWRON);
-	stm32_configgpio(GPIO_OTGFS_OVER);
-	*/
-#endif
-}
-
-/************************************************************************************
- * Name:  stm32_usbsuspend
- *
- * Description:
- *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
- *   used.  This function is called whenever the USB enters or leaves suspend mode.
- *   This is an opportunity for the board logic to shutdown clocks, power, etc.
- *   while the USB is suspended.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
-{
-	ulldbg("resume: %d\n", resume);
-}
-
diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk
deleted file mode 100644
index 99d37eeca..000000000
--- a/src/drivers/boards/px4fmuv2/module.mk
+++ /dev/null
@@ -1,10 +0,0 @@
-#
-# Board-specific startup code for the PX4FMUv2
-#
-
-SRCS		 = px4fmu_can.c \
-		   px4fmu2_init.c \
-		   px4fmu_pwm_servo.c \
-		   px4fmu_spi.c \
-		   px4fmu_usb.c \
-		   px4fmu2_led.c
diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_init.c b/src/drivers/boards/px4fmuv2/px4fmu2_init.c
deleted file mode 100644
index 13829d5c4..000000000
--- a/src/drivers/boards/px4fmuv2/px4fmu2_init.c
+++ /dev/null
@@ -1,262 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_init.c
- *
- * PX4FMU-specific early startup code.  This file implements the
- * nsh_archinitialize() function that is called early by nsh during startup.
- *
- * Code here is run before the rcS script is invoked; it should start required
- * subsystems and perform board-specific initialisation.
- */
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdbool.h>
-#include <stdio.h>
-#include <debug.h>
-#include <errno.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/spi.h>
-#include <nuttx/i2c.h>
-#include <nuttx/sdio.h>
-#include <nuttx/mmcsd.h>
-#include <nuttx/analog/adc.h>
-
-#include <stm32.h>
-#include "px4fmu_internal.h"
-#include <stm32_uart.h>
-
-#include <arch/board/board.h>
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_led.h>
-
-#include <systemlib/cpuload.h>
-
-/****************************************************************************
- * Pre-Processor Definitions
- ****************************************************************************/
-
-/* Configuration ************************************************************/
-
-/* Debug ********************************************************************/
-
-#ifdef CONFIG_CPP_HAVE_VARARGS
-#  ifdef CONFIG_DEBUG
-#    define message(...) lowsyslog(__VA_ARGS__)
-#  else
-#    define message(...) printf(__VA_ARGS__)
-#  endif
-#else
-#  ifdef CONFIG_DEBUG
-#    define message lowsyslog
-#  else
-#    define message printf
-#  endif
-#endif
-
-/****************************************************************************
- * Protected Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/************************************************************************************
- * Name: stm32_boardinitialize
- *
- * Description:
- *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
- *   and mapped but before any devices have been initialized.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_boardinitialize(void)
-{
-	/* configure SPI interfaces */
-	stm32_spiinitialize();
-
-	/* configure LEDs */
-	up_ledinit();
-}
-
-/****************************************************************************
- * Name: nsh_archinitialize
- *
- * Description:
- *   Perform architecture specific initialization
- *
- ****************************************************************************/
-
-static struct spi_dev_s *spi1;
-static struct spi_dev_s *spi2;
-static struct sdio_dev_s *sdio;
-
-#include <math.h>
-
-#ifdef __cplusplus
-__EXPORT int matherr(struct __exception *e)
-{
-	return 1;
-}
-#else
-__EXPORT int matherr(struct exception *e)
-{
-	return 1;
-}
-#endif
-
-__EXPORT int nsh_archinitialize(void)
-{
-
-	/* configure ADC pins */
-	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
-	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
-	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
-	stm32_configgpio(GPIO_ADC1_IN10);	/* unrouted */
-	stm32_configgpio(GPIO_ADC1_IN11);	/* unrouted */
-	stm32_configgpio(GPIO_ADC1_IN12);	/* unrouted */
-	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
-	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
-	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
-
-	/* configure power supply control/sense pins */
-	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
-	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
-	stm32_configgpio(GPIO_VDD_BRICK_VALID);
-	stm32_configgpio(GPIO_VDD_SERVO_VALID);
-	stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
-	stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
-
-	/* configure the high-resolution time/callout interface */
-	hrt_init();
-
-	/* configure CPU load estimation */
-#ifdef CONFIG_SCHED_INSTRUMENTATION
-	cpuload_initialize_once();
-#endif
-
-	/* set up the serial DMA polling */
-	static struct hrt_call serial_dma_call;
-	struct timespec ts;
-
-	/*
-	 * Poll at 1ms intervals for received bytes that have not triggered
-	 * a DMA event.
-	 */
-	ts.tv_sec = 0;
-	ts.tv_nsec = 1000000;
-
-	hrt_call_every(&serial_dma_call,
-		       ts_to_abstime(&ts),
-		       ts_to_abstime(&ts),
-		       (hrt_callout)stm32_serial_dma_poll,
-		       NULL);
-
-	/* initial LED state */
-	drv_led_start();
-	led_off(LED_AMBER);
-
-	/* Configure SPI-based devices */
-
-	spi1 = up_spiinitialize(1);
-
-	if (!spi1) {
-		message("[boot] FAILED to initialize SPI port 1\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
-	SPI_SETFREQUENCY(spi1, 10000000);
-	SPI_SETBITS(spi1, 8);
-	SPI_SETMODE(spi1, SPIDEV_MODE3);
-	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
-	SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
-	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
-	up_udelay(20);
-
-	message("[boot] Successfully initialized SPI port 1\n");
-
-	/* Get the SPI port for the FRAM */
-
-	spi2 = up_spiinitialize(2);
-
-	if (!spi2) {
-		message("[boot] FAILED to initialize SPI port 2\n");
-		up_ledon(LED_AMBER);
-		return -ENODEV;
-	}
-
-	/* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */
-	SPI_SETFREQUENCY(spi2, 375000000);
-	SPI_SETBITS(spi2, 8);
-	SPI_SETMODE(spi2, SPIDEV_MODE3);
-	SPI_SELECT(spi2, SPIDEV_FLASH, false);
-
-	message("[boot] Successfully initialized SPI port 2\n");
-
-	#ifdef CONFIG_MMCSD
-	/* First, get an instance of the SDIO interface */
-
-	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
-	if (!sdio) {
-		message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
-			CONFIG_NSH_MMCSDSLOTNO);
-		return -ENODEV;
-	}
-
-	/* Now bind the SDIO interface to the MMC/SD driver */
-	int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
-	if (ret != OK) {
-		message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
-		return ret;
-	}
-
-	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
-	sdio_mediachange(sdio, true);
-
-	message("[boot] Initialized SDIO\n");
-	#endif
-
-	return OK;
-}
diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c
deleted file mode 100644
index 11a5c7211..000000000
--- a/src/drivers/boards/px4fmuv2/px4fmu2_led.c
+++ /dev/null
@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file px4fmu2_led.c
- *
- * PX4FMU LED backend.
- */
-
-#include <nuttx/config.h>
-
-#include <stdbool.h>
-
-#include "stm32.h"
-#include "px4fmu_internal.h"
-
-#include <arch/board/board.h>
-
-/*
- * Ideally we'd be able to get these from up_internal.h,
- * but since we want to be able to disable the NuttX use
- * of leds for system indication at will and there is no
- * separate switch, we need to build independent of the
- * CONFIG_ARCH_LEDS configuration switch.
- */
-__BEGIN_DECLS
-extern void led_init();
-extern void led_on(int led);
-extern void led_off(int led);
-__END_DECLS
-
-__EXPORT void led_init()
-{
-	/* Configure LED1 GPIO for output */
-
-	stm32_configgpio(GPIO_LED1);
-}
-
-__EXPORT void led_on(int led)
-{
-	if (led == 1)
-	{
-		/* Pull down to switch on */
-		stm32_gpiowrite(GPIO_LED1, false);
-	}
-}
-
-__EXPORT void led_off(int led)
-{
-	if (led == 1)
-	{
-		/* Pull up to switch off */
-		stm32_gpiowrite(GPIO_LED1, true);
-	}
-}
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_can.c b/src/drivers/boards/px4fmuv2/px4fmu_can.c
deleted file mode 100644
index 86ba183b8..000000000
--- a/src/drivers/boards/px4fmuv2/px4fmu_can.c
+++ /dev/null
@@ -1,144 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_can.c
- *
- * Board-specific CAN functions.
- */
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <errno.h>
-#include <debug.h>
-
-#include <nuttx/can.h>
-#include <arch/board/board.h>
-
-#include "chip.h"
-#include "up_arch.h"
-
-#include "stm32.h"
-#include "stm32_can.h"
-#include "px4fmu_internal.h"
-
-#ifdef CONFIG_CAN
-
-/************************************************************************************
- * Pre-processor Definitions
- ************************************************************************************/
-/* Configuration ********************************************************************/
-
-#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
-#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
-#  undef CONFIG_STM32_CAN2
-#endif
-
-#ifdef CONFIG_STM32_CAN1
-#  define CAN_PORT 1
-#else
-#  define CAN_PORT 2
-#endif
-
-/* Debug ***************************************************************************/
-/* Non-standard debug that may be enabled just for testing CAN */
-
-#ifdef CONFIG_DEBUG_CAN
-#  define candbg    dbg
-#  define canvdbg   vdbg
-#  define canlldbg  lldbg
-#  define canllvdbg llvdbg
-#else
-#  define candbg(x...)
-#  define canvdbg(x...)
-#  define canlldbg(x...)
-#  define canllvdbg(x...)
-#endif
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: can_devinit
- *
- * Description:
- *   All STM32 architectures must provide the following interface to work with
- *   examples/can.
- *
- ************************************************************************************/
-
-int can_devinit(void)
-{
-	static bool initialized = false;
-	struct can_dev_s *can;
-	int ret;
-
-	/* Check if we have already initialized */
-
-	if (!initialized) {
-		/* Call stm32_caninitialize() to get an instance of the CAN interface */
-
-		can = stm32_caninitialize(CAN_PORT);
-
-		if (can == NULL) {
-			candbg("ERROR:  Failed to get CAN interface\n");
-			return -ENODEV;
-		}
-
-		/* Register the CAN driver at "/dev/can0" */
-
-		ret = can_register("/dev/can0", can);
-
-		if (ret < 0) {
-			candbg("ERROR: can_register failed: %d\n", ret);
-			return ret;
-		}
-
-		/* Now we are initialized */
-
-		initialized = true;
-	}
-
-	return OK;
-}
-
-#endif
\ No newline at end of file
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h
deleted file mode 100644
index ad66ce563..000000000
--- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h
+++ /dev/null
@@ -1,143 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_internal.h
- *
- * PX4FMU internal definitions
- */
-
-#pragma once
-
-/****************************************************************************************************
- * Included Files
- ****************************************************************************************************/
-
-#include <nuttx/config.h>
-#include <nuttx/compiler.h>
-#include <stdint.h>
-
-__BEGIN_DECLS
-
-/* these headers are not C++ safe */
-#include <stm32.h>
-
- 
-/****************************************************************************************************
- * Definitions
- ****************************************************************************************************/
-/* Configuration ************************************************************************************/
-
-/* PX4IO connection configuration */
-#define PX4IO_SERIAL_DEVICE	"/dev/ttyS4"
-#define PX4IO_SERIAL_TX_GPIO	GPIO_USART6_TX
-#define PX4IO_SERIAL_RX_GPIO	GPIO_USART6_RX
-#define PX4IO_SERIAL_BASE	STM32_USART6_BASE	/* hardwired on the board */
-#define PX4IO_SERIAL_VECTOR	STM32_IRQ_USART6
-#define PX4IO_SERIAL_TX_DMAMAP	DMAMAP_USART6_TX_2
-#define PX4IO_SERIAL_RX_DMAMAP	DMAMAP_USART6_RX_2
-#define PX4IO_SERIAL_CLOCK	STM32_PCLK2_FREQUENCY
-#define PX4IO_SERIAL_BITRATE	1500000			/* 1.5Mbps -> max rate for IO */
-
-
-/* PX4FMU GPIOs ***********************************************************************************/
-/* LEDs */
-
-#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
-
-/* External interrupts */
-
-/* SPI chip selects */
-#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
-#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
-#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
-#define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
-
-/* User GPIOs
- *
- * GPIO0-5 are the PWM servo outputs.
- */
-#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
-#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
-#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
-#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
-#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
-#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
-#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
-#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
-#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
-#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
-#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
-#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
-
-/* Power supply control and monitoring GPIOs */
-#define GPIO_VDD_5V_PERIPH_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
-#define GPIO_VDD_BRICK_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
-#define GPIO_VDD_SERVO_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7)
-#define GPIO_VDD_3V3_SENSORS_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
-#define GPIO_VDD_5V_HIPOWER_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10)
-#define GPIO_VDD_5V_PERIPH_OC	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
-
-/* USB OTG FS
- *
- * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
- */
-#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
-
-/****************************************************************************************************
- * Public Types
- ****************************************************************************************************/
-
-/****************************************************************************************************
- * Public data
- ****************************************************************************************************/
-
-#ifndef __ASSEMBLY__
-
-/****************************************************************************************************
- * Public Functions
- ****************************************************************************************************/
-
-/****************************************************************************************************
- * Name: stm32_spiinitialize
- *
- * Description:
- *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
- *
- ****************************************************************************************************/
-
-extern void stm32_spiinitialize(void);
-
-#endif /* __ASSEMBLY__ */
-
-__END_DECLS
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c
deleted file mode 100644
index bcbc0010c..000000000
--- a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c
+++ /dev/null
@@ -1,105 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_pwm_servo.c
- *
- * Configuration data for the stm32 pwm_servo driver.
- *
- * Note that these arrays must always be fully-sized.
- */
-
-#include <stdint.h>
-
-#include <drivers/stm32/drv_pwm_servo.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_pwm_output.h>
-
-#include <stm32.h>
-#include <stm32_gpio.h>
-#include <stm32_tim.h>
-
-__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
-	{
-		.base = STM32_TIM1_BASE,
-		.clock_register = STM32_RCC_APB2ENR,
-		.clock_bit = RCC_APB2ENR_TIM1EN,
-		.clock_freq = STM32_APB2_TIM1_CLKIN
-	},
-	{
-		.base = STM32_TIM4_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM4EN,
-		.clock_freq = STM32_APB1_TIM4_CLKIN
-	}
-};
-
-__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
-	{
-		.gpio = GPIO_TIM1_CH4OUT,
-		.timer_index = 0,
-		.timer_channel = 4,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM1_CH3OUT,
-		.timer_index = 0,
-		.timer_channel = 3,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM1_CH2OUT,
-		.timer_index = 0,
-		.timer_channel = 2,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM1_CH1OUT,
-		.timer_index = 0,
-		.timer_channel = 1,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM4_CH2OUT,
-		.timer_index = 1,
-		.timer_channel = 2,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM4_CH3OUT,
-		.timer_index = 1,
-		.timer_channel = 3,
-		.default_value = 1000,
-	}
-};
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_spi.c b/src/drivers/boards/px4fmuv2/px4fmu_spi.c
deleted file mode 100644
index 5e90c227d..000000000
--- a/src/drivers/boards/px4fmuv2/px4fmu_spi.c
+++ /dev/null
@@ -1,141 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_spi.c
- *
- * Board-specific SPI functions.
- */
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <nuttx/spi.h>
-#include <arch/board/board.h>
-
-#include <up_arch.h>
-#include <chip.h>
-#include <stm32.h>
-#include "px4fmu_internal.h"
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: stm32_spiinitialize
- *
- * Description:
- *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
- *
- ************************************************************************************/
-
-__EXPORT void weak_function stm32_spiinitialize(void)
-{
-#ifdef CONFIG_STM32_SPI1
-	stm32_configgpio(GPIO_SPI_CS_GYRO);
-	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
-	stm32_configgpio(GPIO_SPI_CS_BARO);
-
-	/* De-activate all peripherals,
-	 * required for some peripheral
-	 * state machines
-	 */
-	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-#endif
-
-#ifdef CONFIG_STM32_SPI2
-	stm32_configgpio(GPIO_SPI_CS_FRAM);
-	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
-#endif
-}
-
-__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
-{
-	/* SPI select is active low, so write !selected to select the device */
-
-	switch (devid) {
-	case PX4_SPIDEV_GYRO:
-		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		break;
-
-	case PX4_SPIDEV_ACCEL_MAG:
-		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
-		break;
-
-	case PX4_SPIDEV_BARO:
-		/* Making sure the other peripherals are not selected */
-		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
-		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
-		break;
-
-	default:
-		break;
-	}
-}
-
-__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
-{
-	return SPI_STATUS_PRESENT;
-}
-
-
-#ifdef CONFIG_STM32_SPI2
-__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
-{
-	/* there can only be one device on this bus, so always select it */
-	stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
-}
-
-__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
-{
-	/* FRAM is always present */
-	return SPI_STATUS_PRESENT;
-}
-#endif
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_usb.c b/src/drivers/boards/px4fmuv2/px4fmu_usb.c
deleted file mode 100644
index 3492e2c68..000000000
--- a/src/drivers/boards/px4fmuv2/px4fmu_usb.c
+++ /dev/null
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- *   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 px4fmu_usb.c
- *
- * Board-specific USB functions.
- */
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <debug.h>
-
-#include <nuttx/usb/usbdev.h>
-#include <nuttx/usb/usbdev_trace.h>
-
-#include <up_arch.h>
-#include <stm32.h>
-#include "px4fmu_internal.h"
-
-/************************************************************************************
- * Definitions
- ************************************************************************************/
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: stm32_usbinitialize
- *
- * Description:
- *   Called to setup USB-related GPIO pins for the PX4FMU board.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_usbinitialize(void)
-{
-	/* The OTG FS has an internal soft pull-up */
-
-	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
-
-#ifdef CONFIG_STM32_OTGFS
-	stm32_configgpio(GPIO_OTGFS_VBUS);
-	/* XXX We only support device mode
-	stm32_configgpio(GPIO_OTGFS_PWRON);
-	stm32_configgpio(GPIO_OTGFS_OVER);
-	*/
-#endif
-}
-
-/************************************************************************************
- * Name:  stm32_usbsuspend
- *
- * Description:
- *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
- *   used.  This function is called whenever the USB enters or leaves suspend mode.
- *   This is an opportunity for the board logic to shutdown clocks, power, etc.
- *   while the USB is suspended.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
-{
-	ulldbg("resume: %d\n", resume);
-}
-
diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h
new file mode 100644
index 000000000..48aadbd76
--- /dev/null
+++ b/src/drivers/boards/px4io-v1/board_config.h
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   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 board_config.h
+ * 
+ * PX4IO hardware definitions.
+ */
+
+#pragma once
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* PX4IO GPIOs **********************************************************************/
+/* LEDs */
+
+#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+
+/* Safety switch button *************************************************************/
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ************************************************************/
+
+#define GPIO_ACC1_PWR_EN  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_ACC2_PWR_EN  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15)
+
+#define GPIO_ACC_OC_DETECT   (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13)
+
+#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
+#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11)
+
+/* Analog inputs ********************************************************************/
+
+#define GPIO_ADC_VBATT	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+#define GPIO_ADC_IN5	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+
+/* 
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
+#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk
new file mode 100644
index 000000000..2601a1c15
--- /dev/null
+++ b/src/drivers/boards/px4io-v1/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IO
+#
+
+SRCS	= px4io_init.c \
+	  px4io_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c
new file mode 100644
index 000000000..8292da9e1
--- /dev/null
+++ b/src/drivers/boards/px4io-v1/px4io_init.c
@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   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 px4io_init.c
+ *
+ * PX4IO-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+
+#include "stm32.h"
+#include "board_config.h"
+#include "stm32_uart.h"
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_pwm_output.h>
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	/* configure GPIOs */
+	stm32_configgpio(GPIO_ACC1_PWR_EN);
+	stm32_configgpio(GPIO_ACC2_PWR_EN);
+	stm32_configgpio(GPIO_SERVO_PWR_EN);
+	stm32_configgpio(GPIO_RELAY1_EN);
+	stm32_configgpio(GPIO_RELAY2_EN);
+
+    	/* turn off - all leds are active low */
+	stm32_gpiowrite(GPIO_LED1, true);
+	stm32_gpiowrite(GPIO_LED2, true);
+	stm32_gpiowrite(GPIO_LED3, true);
+
+	/* LED config */
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
+	stm32_configgpio(GPIO_LED3);
+
+	stm32_configgpio(GPIO_ACC_OC_DETECT);
+	stm32_configgpio(GPIO_SERVO_OC_DETECT);
+	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	stm32_configgpio(GPIO_ADC_VBATT);
+	stm32_configgpio(GPIO_ADC_IN5);
+}
diff --git a/src/drivers/boards/px4io-v1/px4io_pwm_servo.c b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c
new file mode 100644
index 000000000..6df470da6
--- /dev/null
+++ b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *  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 px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 2,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH4OUT,
+		.timer_index = 2,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
new file mode 100644
index 000000000..818b64436
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4iov2_internal.h
+ *
+ * PX4IOV2 internal definitions
+ */
+
+#pragma once
+
+/******************************************************************************
+ * Included Files
+ ******************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+ 
+/******************************************************************************
+ * Definitions
+ ******************************************************************************/
+/* Configuration **************************************************************/
+
+/******************************************************************************
+ * Serial
+ ******************************************************************************/
+#define PX4FMU_SERIAL_BASE	STM32_USART2_BASE
+#define PX4FMU_SERIAL_VECTOR	STM32_IRQ_USART2
+#define PX4FMU_SERIAL_TX_GPIO	GPIO_USART2_TX
+#define PX4FMU_SERIAL_RX_GPIO	GPIO_USART2_RX
+#define PX4FMU_SERIAL_TX_DMA	DMACHAN_USART2_TX
+#define PX4FMU_SERIAL_RX_DMA	DMACHAN_USART2_RX
+#define PX4FMU_SERIAL_CLOCK	STM32_PCLK1_FREQUENCY
+#define PX4FMU_SERIAL_BITRATE	1500000
+
+/******************************************************************************
+ * GPIOS
+ ******************************************************************************/
+
+/* LEDS  **********************************************************************/
+
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+
+/* Safety switch button *******************************************************/
+
+#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
+
+/* Power switch controls ******************************************************/
+
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
+
+/* Analog inputs **************************************************************/
+
+#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+
+/* the same rssi signal goes to both an adc and a timer input */
+#define GPIO_ADC_RSSI   (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
+#define GPIO_TIM_RSSI   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12)
+
+/* PWM pins  **************************************************************/
+
+#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8)
+
+#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
+#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+
+/* SBUS pins  *************************************************************/
+
+/* XXX these should be UART pins */
+#define GPIO_SBUS_INPUT   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_SBUS_OUTPUT  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
+
+/* 
+ * High-resolution timer
+ */
+#define HRT_TIMER		1	/* use timer1 for the HRT */
+#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
+#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 */
+#define GPIO_PPM_IN		GPIO_TIM1_CH1IN
+
+/* LED definitions ******************************************************************/
+/* PX4 has two LEDs that we will encode as: */
+
+#define LED_STARTED       0  /* LED? */
+#define LED_HEAPALLOCATE  1  /* LED? */
+#define LED_IRQSENABLED   2  /* LED? + LED? */
+#define LED_STACKCREATED  3  /* LED? */
+#define LED_INIRQ         4  /* LED? + LED? */
+#define LED_SIGNAL        5  /* LED? + LED? */
+#define LED_ASSERTION     6  /* LED? + LED? + LED? */
+#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
+
diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk
new file mode 100644
index 000000000..85f94e8be
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IOv2
+#
+
+SRCS		= px4iov2_init.c \
+		  px4iov2_pwm_servo.c
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
new file mode 100644
index 000000000..0ea95bded
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   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 px4iov2_init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+
+#include <stm32.h>
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+
+	/* configure GPIOs */
+
+	/* LEDS - default to off */
+	stm32_configgpio(GPIO_LED1);
+	stm32_configgpio(GPIO_LED2);
+	stm32_configgpio(GPIO_LED3);
+
+	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	/* spektrum power enable is active high - disable it by default */
+	/* XXX might not want to do this on warm restart? */
+	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
+	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
+
+	stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
+
+	/* RSSI inputs */
+	stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
+	stm32_configgpio(GPIO_ADC_RSSI);
+
+	/* servo rail voltage */
+	stm32_configgpio(GPIO_ADC_VSERVO);
+
+	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
+
+	stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
+	stm32_configgpio(GPIO_SBUS_OUTPUT);
+	
+	/* sbus output enable is active low - disable it by default */
+	stm32_gpiowrite(GPIO_SBUS_OENABLE, true);
+	stm32_configgpio(GPIO_SBUS_OENABLE);
+
+	stm32_configgpio(GPIO_PPM); /* xxx alternate function */
+
+	stm32_gpiowrite(GPIO_PWM1, false);
+	stm32_configgpio(GPIO_PWM1);
+
+	stm32_gpiowrite(GPIO_PWM2, false);
+	stm32_configgpio(GPIO_PWM2);
+
+	stm32_gpiowrite(GPIO_PWM3, false);
+	stm32_configgpio(GPIO_PWM3);
+
+	stm32_gpiowrite(GPIO_PWM4, false);
+	stm32_configgpio(GPIO_PWM4);
+
+	stm32_gpiowrite(GPIO_PWM5, false);
+	stm32_configgpio(GPIO_PWM5);
+
+	stm32_gpiowrite(GPIO_PWM6, false);
+	stm32_configgpio(GPIO_PWM6);
+
+	stm32_gpiowrite(GPIO_PWM7, false);
+	stm32_configgpio(GPIO_PWM7);
+
+	stm32_gpiowrite(GPIO_PWM8, false);
+	stm32_configgpio(GPIO_PWM8);
+}
diff --git a/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
new file mode 100644
index 000000000..4f1b9487c
--- /dev/null
+++ b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *  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 px4iov2_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_pwm_output.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM2_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM2EN,
+		.clock_freq = STM32_APB1_TIM2_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM2_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM2_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 2,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH4OUT,
+		.timer_index = 2,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1000,
+	}
+};
diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io/module.mk
deleted file mode 100644
index 2601a1c15..000000000
--- a/src/drivers/boards/px4io/module.mk
+++ /dev/null
@@ -1,6 +0,0 @@
-#
-# Board-specific startup code for the PX4IO
-#
-
-SRCS	= px4io_init.c \
-	  px4io_pwm_servo.c
diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c
deleted file mode 100644
index 15c59e423..000000000
--- a/src/drivers/boards/px4io/px4io_init.c
+++ /dev/null
@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- *   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 px4io_init.c
- *
- * PX4IO-specific early startup code.  This file implements the
- * nsh_archinitialize() function that is called early by nsh during startup.
- *
- * Code here is run before the rcS script is invoked; it should start required
- * subsystems and perform board-specific initialisation.
- */
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdbool.h>
-#include <stdio.h>
-#include <debug.h>
-#include <errno.h>
-
-#include <nuttx/arch.h>
-
-#include "stm32.h"
-#include "px4io_internal.h"
-#include "stm32_uart.h"
-
-#include <arch/board/board.h>
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_led.h>
-#include <drivers/drv_pwm_output.h>
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/************************************************************************************
- * Name: stm32_boardinitialize
- *
- * Description:
- *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
- *   and mapped but before any devices have been initialized.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_boardinitialize(void)
-{
-	/* configure GPIOs */
-	stm32_configgpio(GPIO_ACC1_PWR_EN);
-	stm32_configgpio(GPIO_ACC2_PWR_EN);
-	stm32_configgpio(GPIO_SERVO_PWR_EN);
-	stm32_configgpio(GPIO_RELAY1_EN);
-	stm32_configgpio(GPIO_RELAY2_EN);
-
-    	/* turn off - all leds are active low */
-	stm32_gpiowrite(GPIO_LED1, true);
-	stm32_gpiowrite(GPIO_LED2, true);
-	stm32_gpiowrite(GPIO_LED3, true);
-
-	/* LED config */
-	stm32_configgpio(GPIO_LED1);
-	stm32_configgpio(GPIO_LED2);
-	stm32_configgpio(GPIO_LED3);
-
-	stm32_configgpio(GPIO_ACC_OC_DETECT);
-	stm32_configgpio(GPIO_SERVO_OC_DETECT);
-	stm32_configgpio(GPIO_BTN_SAFETY);
-
-	stm32_configgpio(GPIO_ADC_VBATT);
-	stm32_configgpio(GPIO_ADC_IN5);
-}
diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h
deleted file mode 100644
index 6638e715e..000000000
--- a/src/drivers/boards/px4io/px4io_internal.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- *   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 px4io_internal.h
- * 
- * PX4IO hardware definitions.
- */
-
-#pragma once
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-#include <nuttx/compiler.h>
-#include <stdint.h>
-
-#include <stm32.h>
-
-/************************************************************************************
- * Definitions
- ************************************************************************************/
-
-/* PX4IO GPIOs **********************************************************************/
-/* LEDs */
-
-#define GPIO_LED1       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
-                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
-                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
-                         GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-
-/* Safety switch button *************************************************************/
-
-#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
-
-/* Power switch controls ************************************************************/
-
-#define GPIO_ACC1_PWR_EN  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
-#define GPIO_ACC2_PWR_EN  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14)
-#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15)
-
-#define GPIO_ACC_OC_DETECT   (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12)
-#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13)
-
-#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
-#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11)
-
-/* Analog inputs ********************************************************************/
-
-#define GPIO_ADC_VBATT	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
-#define GPIO_ADC_IN5	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c
deleted file mode 100644
index 6df470da6..000000000
--- a/src/drivers/boards/px4io/px4io_pwm_servo.c
+++ /dev/null
@@ -1,123 +0,0 @@
-/****************************************************************************
- *
- *  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 px4fmu_pwm_servo.c
- *
- * Configuration data for the stm32 pwm_servo driver.
- *
- * Note that these arrays must always be fully-sized.
- */
-
-#include <stdint.h>
-
-#include <drivers/stm32/drv_pwm_servo.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_pwm_output.h>
-
-#include <stm32.h>
-#include <stm32_gpio.h>
-#include <stm32_tim.h>
-
-__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
-	{
-		.base = STM32_TIM2_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM2EN,
-		.clock_freq = STM32_APB1_TIM2_CLKIN
-	},
-	{
-		.base = STM32_TIM3_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM3EN,
-		.clock_freq = STM32_APB1_TIM3_CLKIN
-	},
-	{
-		.base = STM32_TIM4_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM4EN,
-		.clock_freq = STM32_APB1_TIM4_CLKIN
-	}
-};
-
-__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
-	{
-		.gpio = GPIO_TIM2_CH1OUT,
-		.timer_index = 0,
-		.timer_channel = 1,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH2OUT,
-		.timer_index = 0,
-		.timer_channel = 2,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM4_CH3OUT,
-		.timer_index = 2,
-		.timer_channel = 3,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM4_CH4OUT,
-		.timer_index = 2,
-		.timer_channel = 4,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM3_CH1OUT,
-		.timer_index = 1,
-		.timer_channel = 1,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM3_CH2OUT,
-		.timer_index = 1,
-		.timer_channel = 2,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM3_CH3OUT,
-		.timer_index = 1,
-		.timer_channel = 3,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM3_CH4OUT,
-		.timer_index = 1,
-		.timer_channel = 4,
-		.default_value = 1000,
-	}
-};
diff --git a/src/drivers/boards/px4iov2/module.mk b/src/drivers/boards/px4iov2/module.mk
deleted file mode 100644
index 85f94e8be..000000000
--- a/src/drivers/boards/px4iov2/module.mk
+++ /dev/null
@@ -1,6 +0,0 @@
-#
-# Board-specific startup code for the PX4IOv2
-#
-
-SRCS		= px4iov2_init.c \
-		  px4iov2_pwm_servo.c
diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4iov2/px4iov2_init.c
deleted file mode 100644
index e95298bf5..000000000
--- a/src/drivers/boards/px4iov2/px4iov2_init.c
+++ /dev/null
@@ -1,162 +0,0 @@
-/****************************************************************************
- *
- *   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 px4iov2_init.c
- *
- * PX4FMU-specific early startup code.  This file implements the
- * nsh_archinitialize() function that is called early by nsh during startup.
- *
- * Code here is run before the rcS script is invoked; it should start required
- * subsystems and perform board-specific initialisation.
- */
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdbool.h>
-#include <stdio.h>
-#include <debug.h>
-#include <errno.h>
-
-#include <nuttx/arch.h>
-
-#include <stm32.h>
-#include "px4iov2_internal.h"
-
-#include <arch/board/board.h>
-
-/****************************************************************************
- * Pre-Processor Definitions
- ****************************************************************************/
-
-/* Configuration ************************************************************/
-
-/* Debug ********************************************************************/
-
-#ifdef CONFIG_CPP_HAVE_VARARGS
-#  ifdef CONFIG_DEBUG
-#    define message(...) lowsyslog(__VA_ARGS__)
-#  else
-#    define message(...) printf(__VA_ARGS__)
-#  endif
-#else
-#  ifdef CONFIG_DEBUG
-#    define message lowsyslog
-#  else
-#    define message printf
-#  endif
-#endif
-
-/****************************************************************************
- * Protected Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/************************************************************************************
- * Name: stm32_boardinitialize
- *
- * Description:
- *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
- *   and mapped but before any devices have been initialized.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_boardinitialize(void)
-{
-
-	/* configure GPIOs */
-
-	/* LEDS - default to off */
-	stm32_configgpio(GPIO_LED1);
-	stm32_configgpio(GPIO_LED2);
-	stm32_configgpio(GPIO_LED3);
-
-	stm32_configgpio(GPIO_BTN_SAFETY);
-
-	/* spektrum power enable is active high - disable it by default */
-	/* XXX might not want to do this on warm restart? */
-	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
-	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
-
-	stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
-
-	/* RSSI inputs */
-	stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */
-	stm32_configgpio(GPIO_ADC_RSSI);
-
-	/* servo rail voltage */
-	stm32_configgpio(GPIO_ADC_VSERVO);
-
-	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
-
-	stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
-	stm32_configgpio(GPIO_SBUS_OUTPUT);
-	
-	/* sbus output enable is active low - disable it by default */
-	stm32_gpiowrite(GPIO_SBUS_OENABLE, true);
-	stm32_configgpio(GPIO_SBUS_OENABLE);
-
-	stm32_configgpio(GPIO_PPM); /* xxx alternate function */
-
-	stm32_gpiowrite(GPIO_PWM1, false);
-	stm32_configgpio(GPIO_PWM1);
-
-	stm32_gpiowrite(GPIO_PWM2, false);
-	stm32_configgpio(GPIO_PWM2);
-
-	stm32_gpiowrite(GPIO_PWM3, false);
-	stm32_configgpio(GPIO_PWM3);
-
-	stm32_gpiowrite(GPIO_PWM4, false);
-	stm32_configgpio(GPIO_PWM4);
-
-	stm32_gpiowrite(GPIO_PWM5, false);
-	stm32_configgpio(GPIO_PWM5);
-
-	stm32_gpiowrite(GPIO_PWM6, false);
-	stm32_configgpio(GPIO_PWM6);
-
-	stm32_gpiowrite(GPIO_PWM7, false);
-	stm32_configgpio(GPIO_PWM7);
-
-	stm32_gpiowrite(GPIO_PWM8, false);
-	stm32_configgpio(GPIO_PWM8);
-}
diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h
deleted file mode 100755
index 2bf65e047..000000000
--- a/src/drivers/boards/px4iov2/px4iov2_internal.h
+++ /dev/null
@@ -1,118 +0,0 @@
-/****************************************************************************
- *
- *   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 px4iov2_internal.h
- *
- * PX4IOV2 internal definitions
- */
-
-#pragma once
-
-/******************************************************************************
- * Included Files
- ******************************************************************************/
-
-#include <nuttx/config.h>
-#include <nuttx/compiler.h>
-#include <stdint.h>
-
-/* these headers are not C++ safe */
-#include <stm32.h>
-
- 
-/******************************************************************************
- * Definitions
- ******************************************************************************/
-/* Configuration **************************************************************/
-
-/******************************************************************************
- * Serial
- ******************************************************************************/
-#define PX4FMU_SERIAL_BASE	STM32_USART2_BASE
-#define PX4FMU_SERIAL_VECTOR	STM32_IRQ_USART2
-#define PX4FMU_SERIAL_TX_GPIO	GPIO_USART2_TX
-#define PX4FMU_SERIAL_RX_GPIO	GPIO_USART2_RX
-#define PX4FMU_SERIAL_TX_DMA	DMACHAN_USART2_TX
-#define PX4FMU_SERIAL_RX_DMA	DMACHAN_USART2_RX
-#define PX4FMU_SERIAL_CLOCK	STM32_PCLK1_FREQUENCY
-#define PX4FMU_SERIAL_BITRATE	1500000
-
-/******************************************************************************
- * GPIOS
- ******************************************************************************/
-
-/* LEDS  **********************************************************************/
-
-#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
-
-/* Safety switch button *******************************************************/
-
-#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
-
-/* Power switch controls ******************************************************/
-
-#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
-
-#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
-
-/* Analog inputs **************************************************************/
-
-#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
-
-/* the same rssi signal goes to both an adc and a timer input */
-#define GPIO_ADC_RSSI   (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
-#define GPIO_TIM_RSSI   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12)
-
-/* PWM pins  **************************************************************/
-
-#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8)
-
-#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
-#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
-#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
-#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
-#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
-
-/* SBUS pins  *************************************************************/
-
-/* XXX these should be UART pins */
-#define GPIO_SBUS_INPUT   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
-#define GPIO_SBUS_OUTPUT  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
-
diff --git a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c
deleted file mode 100644
index 4f1b9487c..000000000
--- a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c
+++ /dev/null
@@ -1,123 +0,0 @@
-/****************************************************************************
- *
- *  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 px4iov2_pwm_servo.c
- *
- * Configuration data for the stm32 pwm_servo driver.
- *
- * Note that these arrays must always be fully-sized.
- */
-
-#include <stdint.h>
-
-#include <drivers/stm32/drv_pwm_servo.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_pwm_output.h>
-
-#include <stm32.h>
-#include <stm32_gpio.h>
-#include <stm32_tim.h>
-
-__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
-	{
-		.base = STM32_TIM2_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM2EN,
-		.clock_freq = STM32_APB1_TIM2_CLKIN
-	},
-	{
-		.base = STM32_TIM3_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM3EN,
-		.clock_freq = STM32_APB1_TIM3_CLKIN
-	},
-	{
-		.base = STM32_TIM4_BASE,
-		.clock_register = STM32_RCC_APB1ENR,
-		.clock_bit = RCC_APB1ENR_TIM4EN,
-		.clock_freq = STM32_APB1_TIM4_CLKIN
-	}
-};
-
-__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
-	{
-		.gpio = GPIO_TIM2_CH1OUT,
-		.timer_index = 0,
-		.timer_channel = 1,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM2_CH2OUT,
-		.timer_index = 0,
-		.timer_channel = 2,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM4_CH3OUT,
-		.timer_index = 2,
-		.timer_channel = 3,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM4_CH4OUT,
-		.timer_index = 2,
-		.timer_channel = 4,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM3_CH1OUT,
-		.timer_index = 1,
-		.timer_channel = 1,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM3_CH2OUT,
-		.timer_index = 1,
-		.timer_channel = 2,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM3_CH3OUT,
-		.timer_index = 1,
-		.timer_channel = 3,
-		.default_value = 1000,
-	},
-	{
-		.gpio = GPIO_TIM3_CH4OUT,
-		.timer_index = 1,
-		.timer_channel = 4,
-		.default_value = 1000,
-	}
-};
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index c7c25048a..37af26d52 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -59,9 +59,7 @@
 # define GPIO_CAN_RX		(1<<7)		/**< CAN2 RX */
 
 /**
- * Default GPIO device - other devices may also support this protocol if
- * they also export GPIO-like things.  This is always the GPIOs on the
- * main board.
+ * Device paths for things that support the GPIO ioctl protocol.
  */
 # define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
 # define PX4IO_DEVICE_PATH	"/dev/px4io"
@@ -89,9 +87,7 @@
 # define GPIO_5V_PERIPH_OC	(1<<11)		/**< PE10 - !VDD_5V_PERIPH_OC */
 
 /**
- * Default GPIO device - other devices may also support this protocol if
- * they also export GPIO-like things.  This is always the GPIOs on the
- * main board.
+ * Device paths for things that support the GPIO ioctl protocol.
  */
 # define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
 # define PX4IO_DEVICE_PATH	"/dev/px4io"
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index de8028b0f..f79b0b1a3 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -59,7 +59,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/airspeed.h>
 #include <systemlib/err.h>
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index ac3bdc132..039b496f4 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -58,7 +58,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 61a38b125..3bb9a7764 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -59,11 +59,10 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-
 #include <drivers/device/spi.h>
 #include <drivers/drv_gyro.h>
 
+#include <board_config.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 844cc3884..d9801f88f 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -59,12 +59,12 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 
+#include <board_config.h>
+
 #include "iirFilter.h"
 
 /* oddly, ERROR is not defined for c++ */
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 397686e8b..c5f49fb36 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -59,8 +59,6 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
-
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
@@ -70,6 +68,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
 
+#include <board_config.h>
+
 /* Configuration Constants */
 #define MB12XX_BUS 			PX4_I2C_BUS_EXPANSION
 #define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 7a2e22c01..b19a1a0e6 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -69,7 +69,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include <systemlib/airspeed.h>
 #include <systemlib/err.h>
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 1f4a63cf3..4ad13fc04 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -59,10 +59,11 @@
 #include <nuttx/arch.h>
 #include <nuttx/i2c.h>
 
+#include <board_config.h>
+
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 1fd6cb17e..0e27a382a 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -63,7 +63,7 @@
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include <drivers/device/spi.h>
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index b73f71572..a6f337486 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -59,15 +59,7 @@
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
-# include <drivers/boards/px4fmu/px4fmu_internal.h>
-# define FMU_HAVE_PPM
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
-# include <drivers/boards/px4fmuv2/px4fmu_internal.h>
-# undef FMU_HAVE_PPM
-#else
-# error Unrecognised FMU board.
-#endif
+# include <board_config.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -80,7 +72,7 @@
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
-#ifdef FMU_HAVE_PPM
+#ifdef HRT_PPM_CHANNEL
 # include <systemlib/ppm_decode.h>
 #endif
 
@@ -455,7 +447,7 @@ PX4FMU::task_main()
 	fds[1].fd = _t_armed;
 	fds[1].events = POLLIN;
 
-#ifdef FMU_HAVE_PPM
+#ifdef HRT_PPM_CHANNEL
 	// rc input, published to ORB
 	struct rc_input_values rc_in;
 	orb_advert_t to_input_rc = 0;
@@ -585,7 +577,7 @@ PX4FMU::task_main()
 			}
 		}
 
-#ifdef FMU_HAVE_PPM
+#ifdef HRT_PPM_CHANNEL
 		// see if we have new PPM input data
 		if (ppm_last_valid_decode != rc_in.timestamp) {
 			// we have a new PPM frame. Publish it.
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index 840b96f5b..236cb918d 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -61,7 +61,7 @@
 
 #include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/boards/px4fmuv2/px4fmu_internal.h>	/* XXX should really not be hardcoding v2 here */
+#include <board_config.h>
 
 #include <systemlib/perf_counter.h>
 
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index ec22a5e17..c7ce60b5e 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -56,12 +56,7 @@
 
 #include "uploader.h"
 
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-#include <drivers/boards/px4fmuv2/px4fmu_internal.h>
-#endif
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
-#endif
+#include <board_config.h>
 
 // define for comms logging
 //#define UDEBUG
diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index dae41d934..5c4fa4bb6 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -41,7 +41,6 @@
 
 #include <nuttx/config.h>
 
-#include <arch/board/board.h>
 #include <drivers/device/i2c.h>
 
 #include <sys/types.h>
@@ -60,6 +59,8 @@
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 
+#include <board_config.h>
+
 #include "device/rgbled.h"
 
 #define LED_ONTIME 120
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 1cc18afda..58529fb03 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -59,7 +59,7 @@
 #include <errno.h>
 #include <string.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include "chip.h"
@@ -70,8 +70,6 @@
 #include "stm32_gpio.h"
 #include "stm32_tim.h"
 
-#ifdef HRT_TIMER
-
 /* HRT configuration */
 #if   HRT_TIMER == 1
 # define HRT_TIMER_BASE		STM32_TIM1_BASE
@@ -905,6 +903,3 @@ hrt_latency_update(void)
 	/* catch-all at the end */
 	latency_counters[index]++;
 }
-
-
-#endif /* HRT_TIMER */
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 2284be84d..24eec52af 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -104,7 +104,7 @@
 #include <math.h>
 #include <ctype.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 #include <drivers/drv_hrt.h>
 
 #include <arch/stm32/chip.h>
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index bd78f2638..9eb092a63 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -42,12 +42,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
-# include <drivers/boards/px4io/px4io_internal.h>
-#endif
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
-# include <drivers/boards/px4iov2/px4iov2_internal.h>
-#endif
+#include <board_config.h>
 
 #include "protocol.h"
 
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index a2b0d8cae..96276b56a 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -43,11 +43,13 @@
 #include <fcntl.h>
 #include <sched.h>
 #include <signal.h>
-#include <sys/stat.h>
 #include <unistd.h>
 #include <float.h>
 #include <string.h>
 
+#include <sys/stat.h>
+#include <sys/types.h>
+
 #include "systemlib.h"
 
 __EXPORT extern void systemreset(void) {
diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c
index 49da51358..2aed80e01 100644
--- a/src/systemcmds/eeprom/eeprom.c
+++ b/src/systemcmds/eeprom/eeprom.c
@@ -55,7 +55,7 @@
 #include <nuttx/fs/nxffs.h>
 #include <nuttx/fs/ioctl.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include "systemlib/systemlib.h"
 #include "systemlib/param/param.h"
diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c
index 4da238039..34405c496 100644
--- a/src/systemcmds/i2c/i2c.c
+++ b/src/systemcmds/i2c/i2c.c
@@ -52,7 +52,7 @@
 
 #include <nuttx/i2c.h>
 
-#include <arch/board/board.h>
+#include <board_config.h>
 
 #include "systemlib/systemlib.h"
 #include "systemlib/err.h"
-- 
cgit v1.2.3


From 9499e48a524941d955c1b092c7b4e8ebdc1cc807 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 4 Aug 2013 01:52:22 +0200
Subject: Fixed setting mag queue depth

---
 src/drivers/lsm303d/lsm303d.cpp | 28 +++++++++++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index d9801f88f..7a65f644a 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -811,8 +811,34 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 			return SENSOR_POLLRATE_MANUAL;
 
 		return 1000000 / _call_mag_interval;
-	case SENSORIOCSQUEUEDEPTH:
+	
+	case SENSORIOCSQUEUEDEPTH: {
+			/* account for sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct mag_report *buf = new struct mag_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _mag_reports;
+			_num_mag_reports = arg;
+			_mag_reports = buf;
+			start();
+
+			return OK;
+		}
+
 	case SENSORIOCGQUEUEDEPTH:
+		return _num_mag_reports - 1;
+
 	case SENSORIOCRESET:
 		return ioctl(filp, cmd, arg);
 
-- 
cgit v1.2.3


From 686edfefb8b8bb90e630cac166ad06776229f55a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 4 Aug 2013 13:46:30 +1000
Subject: Removed LSM303D filter

---
 src/drivers/lsm303d/iirFilter.c | 255 ----------------------------------------
 src/drivers/lsm303d/iirFilter.h |  59 ----------
 src/drivers/lsm303d/lsm303d.cpp |  22 ----
 src/drivers/lsm303d/module.mk   |   5 +-
 4 files changed, 3 insertions(+), 338 deletions(-)
 delete mode 100644 src/drivers/lsm303d/iirFilter.c
 delete mode 100644 src/drivers/lsm303d/iirFilter.h

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/iirFilter.c b/src/drivers/lsm303d/iirFilter.c
deleted file mode 100644
index 8311f14d6..000000000
--- a/src/drivers/lsm303d/iirFilter.c
+++ /dev/null
@@ -1,255 +0,0 @@
-#include "math.h"
-#include "string.h"
-#include "iirFilter.h"
-
-///////////////////////////////////////////////////////////////////////////////
-// Internal function prototypes
-
-int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, double Ts, TF_ZPG_t *pZpgd);
-
-int btDifcToZpgd(const TF_DIF_t *pkDifc, double Ts, TF_ZPG_t *pZpgd);
-
-int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt);
-
-int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly);
-
-///////////////////////////////////////////////////////////////////////////////
-// external functions
-
-int testFunction()
-{
-	printf("TEST\n");
-	return 1;
-}
-
-int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt)
-{
-	TF_POLY_t polyd;
-	TF_ZPG_t zpgd;
-
-	memset(pFilt, 0, sizeof(FIL_T));
-
-	// perform bilinear transform with frequency pre warping
-	btDifcToZpgd(pDifc, Ts, &zpgd);
-
-	// calculate polynom
-	tZpgxToPolyx(&zpgd, &polyd);
-
-	// set the filter parameters
-	tPolydToFil(&polyd, pFilt);
-
-	return 1;
-}
-
-// run filter using inp, return output of the filter
-float updateFilter(FIL_T *pFilt, float inp)
-{
-	float outp = 0;
-	int idx; // index used for different things
-	int i; // loop variable
-
-	// Store the input to the input array
-	idx = pFilt->inpCnt % MAX_LENGTH;
-	pFilt->inpData[idx] = inp;
-
-	// calculate numData * inpData
-	for (i = 0; i < pFilt->numLength; i++)
-	{
-		// index of inp array
-		idx = (pFilt->inpCnt + i - pFilt->numLength + 1) % MAX_LENGTH;
-		outp += pFilt->numData[i] * pFilt->inpData[idx];
-	}
-
-	// calculate denData * outData
-	for (i = 0; i < pFilt->denLength; i++)
-	{
-		// index of inp array
-		idx = (pFilt->inpCnt + i - pFilt->denLength) % MAX_LENGTH;
-		outp -= pFilt->denData[i] * pFilt->outData[idx];
-	}
-
-	// store the ouput data to the output array
-	idx = pFilt->inpCnt % MAX_LENGTH;
-	pFilt->outData[idx] = outp;
-
-	pFilt->inpCnt += 1;
-
-	return outp;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Internal functions
-
-int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt)
-{
-	double gain;
-	int i;
-
-	if (pkPolyd->Ts < 0)
-	{
-		return 0;
-	}
-
-	// initialize filter to 0
-	memset(pFilt, 0, sizeof(FIL_T));
-
-	gain = pkPolyd->denData[pkPolyd->denLength - 1];
-	pFilt->Ts = pkPolyd->Ts;
-
-	pFilt->denLength = pkPolyd->denLength - 1;
-	pFilt->numLength = pkPolyd->numLength;
-
-	for (i = 0; i < pkPolyd->numLength; i++)
-	{
-		pFilt->numData[i] = pkPolyd->numData[i] / gain;
-	}
-
-	for (i = 0; i < (pkPolyd->denLength - 1); i++)
-	{
-		pFilt->denData[i] = pkPolyd->denData[i] / gain;
-	}
-}
-
-// bilinear transformation of poles and zeros
-int btDifcToZpgd(const TF_DIF_t *pkDifc,
-				 double Ts,
-				 TF_ZPG_t *pZpgd)
-{
-	TF_ZPG_t zpgc;
-	int totDiff;
-	int i;
-
-	zpgc.zerosLength = 0;
-	zpgc.polesLength = 0;
-	zpgc.gain = pkDifc->gain;
-	zpgc.Ts = pkDifc->Ts;
-
-	// Total number of differentiators / integerators
-	// positive diff, negative integ, 0 for no int/diff
-	totDiff = pkDifc->numDiff - pkDifc->numInt + pkDifc->highpassLength;
-
-	while (zpgc.zerosLength < totDiff)
-	{
-		zpgc.zerosData[zpgc.zerosLength] = 0;
-		zpgc.zerosLength++;
-	}
-	while (zpgc.polesLength < -totDiff)
-	{
-		zpgc.polesData[zpgc.polesLength] = 0;
-		zpgc.polesLength++;
-	}
-
-	// The next step is to calculate the poles
-	// This has to be done for the highpass and lowpass filters
-	// As we are using bilinear transformation there will be frequency
-	// warping which has to be accounted for
-	for (i = 0; i < pkDifc->lowpassLength; i++)
-	{
-		// pre-warping:
-		double freq = 2.0 / Ts * tan(pkDifc->lowpassData[i] * 2.0 * M_PI * Ts / 2.0);
-		// calculate pole:
-		zpgc.polesData[zpgc.polesLength] = -freq;
-		// adjust gain such that lp has gain = 1 for low frequencies
-		zpgc.gain *= freq;
-		zpgc.polesLength++;
-	}
-	for (i = 0; i < pkDifc->highpassLength; i++)
-	{
-		// pre-warping
-		double freq = 2.0 / Ts * tan(pkDifc->highpassData[i] * 2.0 * M_PI * Ts / 2.0);
-		// calculate pole:
-		zpgc.polesData[zpgc.polesLength] = -freq;
-		// gain does not have to be adjusted
-		zpgc.polesLength++;
-	}
-
-	return btZpgcToZpgd(&zpgc, Ts, pZpgd);
-}
-
-// bilinear transformation of poles and zeros
-int btZpgcToZpgd(const TF_ZPG_t *pkZpgc,
-				 double Ts,
-				 TF_ZPG_t *pZpgd)
-{
-	int i;
-
-	// init digital gain
-	pZpgd->gain = pkZpgc->gain;
-
-	// transform the poles
-	pZpgd->polesLength = pkZpgc->polesLength;
-	for (i = 0; i < pkZpgc->polesLength; i++)
-	{
-		pZpgd->polesData[i] = (2.0 / Ts + pkZpgc->polesData[i]) / (2.0 / Ts - pkZpgc->polesData[i]);
-		pZpgd->gain /= (2.0 / Ts - pkZpgc->polesData[i]);
-	}
-
-	// transform the zeros
-	pZpgd->zerosLength = pkZpgc->zerosLength;
-	for (i = 0; i < pkZpgc->zerosLength; i++)
-	{
-		pZpgd->zerosData[i] = (2.0 / Ts + pkZpgc->zerosData[i]) / (2.0 / Ts + pkZpgc->zerosData[i]);
-		pZpgd->gain *= (2.0 / Ts - pkZpgc->zerosData[i]);
-	}
-
-	// if we don't have the same number of poles as zeros we have to add
-	// poles or zeros due to the bilinear transformation
-	while (pZpgd->zerosLength < pZpgd->polesLength)
-	{
-		pZpgd->zerosData[pZpgd->zerosLength] = -1.0;
-		pZpgd->zerosLength++;
-	}
-	while (pZpgd->zerosLength > pZpgd->polesLength)
-	{
-		pZpgd->polesData[pZpgd->polesLength] = -1.0;
-		pZpgd->polesLength++;
-	}
-
-	pZpgd->Ts = Ts;
-
-	return 1;
-}
-
-// calculate polynom from zero, pole, gain form
-int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly)
-{
-	int i, j; // counter variable
-	double tmp0, tmp1, gain;
-
-	// copy Ts
-	pPoly->Ts = pkZpg->Ts;
-
-	// calculate denominator polynom
-	pPoly->denLength = 1;
-	pPoly->denData[0] = 1.0;
-	for (i = 0; i < pkZpg->polesLength; i++)
-	{
-		// init temporary variable
-		tmp0 = 0.0;
-		// increase the polynom by 1
-		pPoly->denData[pPoly->denLength] = 0;
-		pPoly->denLength++;
-		for (j = 0; j < pPoly->denLength; j++)
-		{
-			tmp1 = pPoly->denData[j];
-			pPoly->denData[j] = tmp1 * -pkZpg->polesData[i] + tmp0;
-			tmp0 = tmp1;
-		}
-	}
-
-	// calculate numerator polynom
-	pPoly->numLength = 1;
-	pPoly->numData[0] = pkZpg->gain;
-	for (i = 0; i < pkZpg->zerosLength; i++)
-	{
-		tmp0 = 0.0;
-		pPoly->numData[pPoly->numLength] = 0;
-		pPoly->numLength++;
-		for (j = 0; j < pPoly->numLength; j++)
-		{
-			tmp1 = pPoly->numData[j];
-			pPoly->numData[j] = tmp1 * -pkZpg->zerosData[i] + tmp0;
-			tmp0 = tmp1;
-		}
-	}
-}
diff --git a/src/drivers/lsm303d/iirFilter.h b/src/drivers/lsm303d/iirFilter.h
deleted file mode 100644
index ab4223a8e..000000000
--- a/src/drivers/lsm303d/iirFilter.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#ifndef IIRFILTER__H__
-#define IIRFILTER__H__
-
-__BEGIN_DECLS
-
-#define MAX_LENGTH 4
-
-typedef struct FILTER_s
-{
-	float denData[MAX_LENGTH];
-	float numData[MAX_LENGTH];
-	int denLength;
-	int numLength;
-	float Ts;
-	float inpData[MAX_LENGTH];
-	float outData[MAX_LENGTH];
-	unsigned int inpCnt;
-} FIL_T;
-
-typedef struct TF_ZPG_s
-{
-	int    zerosLength;
-	double zerosData[MAX_LENGTH + 1];
-	int    polesLength;
-	double polesData[MAX_LENGTH + 1];
-	double gain;
-	double Ts;
-} TF_ZPG_t;
-
-typedef struct TF_POLY_s
-{
-	int    numLength;
-	double numData[MAX_LENGTH];
-	int    denLength;
-	double denData[MAX_LENGTH];
-	double Ts;
-} TF_POLY_t;
-
-typedef struct TF_DIF_s
-{
-	int    numInt;
-	int    numDiff;
-	int    lowpassLength;
-	int    highpassLength;
-	double lowpassData[MAX_LENGTH];
-	int    highpassData[MAX_LENGTH];
-	double gain;
-	double Ts;
-} TF_DIF_t;
-
-__EXPORT int testFunction(void);
-
-__EXPORT float updateFilter(FIL_T *pFilt, float inp);
-
-__EXPORT int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt);
-
-__END_DECLS
-
-#endif
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 7a65f644a..9ebffcac9 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -65,8 +65,6 @@
 
 #include <board_config.h>
 
-#include "iirFilter.h"
-
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -221,10 +219,6 @@ private:
 
 	unsigned			_current_samplerate;
 
-//	FIL_T				_filter_x;
-//	FIL_T				_filter_y;
-//	FIL_T				_filter_z;
-
 	unsigned			_num_mag_reports;
 	volatile unsigned	_next_mag_report;
 	volatile unsigned	_oldest_mag_report;
@@ -494,22 +488,6 @@ LSM303D::init()
 	set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
 	set_samplerate(400); /* max sample rate */
 
-	/* initiate filter */
-//	TF_DIF_t tf_filter;
-//	tf_filter.numInt = 0;
-//	tf_filter.numDiff = 0;
-//	tf_filter.lowpassLength = 2;
-//	tf_filter.highpassLength = 0;
-//	tf_filter.lowpassData[0] = 10;
-//	tf_filter.lowpassData[1] = 10;
-//	//tf_filter.highpassData[0] = ;
-//	tf_filter.gain = 1;
-//	tf_filter.Ts = 1/_current_samplerate;
-//
-//	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x);
-//	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y);
-//	initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z);
-
 	mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
 	mag_set_samplerate(100);
 
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
index 8fd5679c9..e40f718c5 100644
--- a/src/drivers/lsm303d/module.mk
+++ b/src/drivers/lsm303d/module.mk
@@ -3,5 +3,6 @@
 #
 
 MODULE_COMMAND	 = lsm303d
-SRCS		 = lsm303d.cpp \
-		iirFilter.c
+SRCS		 = lsm303d.cpp
+
+
-- 
cgit v1.2.3


From 26204496b69740bddfd6f8ddbdd71ee4e755008c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 4 Aug 2013 15:10:42 +1000
Subject: Merged LSM303D lowpass

---
 src/drivers/lsm303d/lsm303d.cpp | 55 ++++++++++++++++++++++++-----------------
 1 file changed, 33 insertions(+), 22 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 9ebffcac9..56400c843 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -64,6 +64,7 @@
 #include <drivers/drv_mag.h>
 
 #include <board_config.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -232,6 +233,10 @@ private:
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
 
+        math::LowPassFilter2p   _accel_filter_x;
+        math::LowPassFilter2p   _accel_filter_y;
+        math::LowPassFilter2p   _accel_filter_z;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -402,7 +407,10 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_range_scale(0.0f),
 	_mag_range_ga(0.0f),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
-	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read"))
+        _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
+        _accel_filter_x(800, 30),
+        _accel_filter_y(800, 30),
+        _accel_filter_z(800, 30)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -446,7 +454,6 @@ LSM303D::init()
 {
 	int ret = ERROR;
 	int mag_ret;
-	int fd_mag;
 
 	/* do SPI init (and probe) first */
 	if (SPI::init() != OK)
@@ -622,7 +629,6 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
-
 				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
 
 			case SENSOR_POLLRATE_DEFAULT:
@@ -645,6 +651,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 					/* adjust sample rate of sensor */
 					set_samplerate(arg);
 
+                                        // adjust filters
+                                        float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
+                                        float sample_rate = 1.0e6f/ticks;
+                                        _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+                                        _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+                                        _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_accel_call.period = _call_accel_interval = ticks;
@@ -695,15 +708,17 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* XXX implement */
 		return -EINVAL;
 
-//	case ACCELIOCSLOWPASS:
-	case ACCELIOCGLOWPASS:
+	case ACCELIOCSLOWPASS: {
+                float cutoff_freq_hz = arg;
+                float sample_rate = 1.0e6f / _call_accel_interval;
+                _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+                _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+                _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+                return OK;
+        }
 
-		unsigned bandwidth;
-
-		if (OK == get_antialias_filter_bandwidth(bandwidth))
-			return bandwidth;
-		else
-			return -EINVAL;
+	case ACCELIOCGLOWPASS:
+                return _accel_filter_x.get_cutoff_freq();
 
 	case ACCELIOCSSCALE:
 		{
@@ -1186,17 +1201,13 @@ LSM303D::measure()
 	accel_report->y_raw = raw_accel_report.y;
 	accel_report->z_raw = raw_accel_report.z;
 
-//	float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-//	float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-//	float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-//
-//	accel_report->x = updateFilter(&_filter_x, x_in_new);
-//	accel_report->y = updateFilter(&_filter_y, y_in_new);
-//	accel_report->z = updateFilter(&_filter_z, z_in_new);
-
-	accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+
+	accel_report->x = _accel_filter_x.apply(x_in_new);
+	accel_report->y = _accel_filter_y.apply(y_in_new);
+	accel_report->z = _accel_filter_z.apply(z_in_new);
 
 	accel_report->scaling = _accel_range_scale;
 	accel_report->range_m_s2 = _accel_range_m_s2;
-- 
cgit v1.2.3


From ca9a11a586e8c4994e8be28e1b0e23c3f0d341ed Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 5 Aug 2013 23:09:02 +0200
Subject: Indendation fixes

---
 src/drivers/l3gd20/l3gd20.cpp   |  6 +++---
 src/drivers/lsm303d/lsm303d.cpp | 14 +++++++-------
 2 files changed, 10 insertions(+), 10 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index ce381807f..fde201351 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -189,9 +189,9 @@ private:
 
 	perf_counter_t		_sample_perf;
 
-        math::LowPassFilter2p   _gyro_filter_x;
-        math::LowPassFilter2p   _gyro_filter_y;
-        math::LowPassFilter2p   _gyro_filter_z;
+	math::LowPassFilter2p	_gyro_filter_x;
+	math::LowPassFilter2p	_gyro_filter_y;
+	math::LowPassFilter2p	_gyro_filter_z;
 
 	/**
 	 * Start automatic measurement.
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 56400c843..a95b62468 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -233,9 +233,9 @@ private:
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
 
-        math::LowPassFilter2p   _accel_filter_x;
-        math::LowPassFilter2p   _accel_filter_y;
-        math::LowPassFilter2p   _accel_filter_z;
+	math::LowPassFilter2p	_accel_filter_x;
+	math::LowPassFilter2p	_accel_filter_y;
+	math::LowPassFilter2p	_accel_filter_z;
 
 	/**
 	 * Start automatic measurement.
@@ -407,10 +407,10 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_range_scale(0.0f),
 	_mag_range_ga(0.0f),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
-        _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
-        _accel_filter_x(800, 30),
-        _accel_filter_y(800, 30),
-        _accel_filter_z(800, 30)
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
+	_accel_filter_x(800, 30),
+	_accel_filter_y(800, 30),
+	_accel_filter_z(800, 30)
 {
 	// enable debug() calls
 	_debug_enabled = true;
-- 
cgit v1.2.3


From cbb5ce8f59a34615fe0d702041c497efe40edb56 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 11 Aug 2013 16:54:00 +0200
Subject: Fixed startup behavior for PX4 autostart

---
 ROMFS/px4fmu_common/init.d/10_io_f330 | 10 ++++------
 ROMFS/px4fmu_common/init.d/rc.sensors |  3 ---
 ROMFS/px4fmu_common/init.d/rc.usb     | 14 ++++++++++++++
 src/drivers/l3gd20/l3gd20.cpp         |  2 +-
 src/drivers/lsm303d/lsm303d.cpp       |  2 +-
 5 files changed, 20 insertions(+), 11 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330
index 4450eb50d..4083bb905 100644
--- a/ROMFS/px4fmu_common/init.d/10_io_f330
+++ b/ROMFS/px4fmu_common/init.d/10_io_f330
@@ -75,6 +75,8 @@ px4io min 1200 1200 1200 1200
 # Upper limits could be higher, this is on the safe side
 #
 px4io max 1800 1800 1800 1800
+
+mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
  
 #
 # Start the sensors (depends on orb, px4io)
@@ -95,17 +97,13 @@ gps start
 # Start the attitude estimator (depends on orb)
 #
 attitude_estimator_ekf start
- 
-#
-# Load mixer and start controllers (depends on px4io)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+
 multirotor_att_control start
  
 #
 # Start logging
 #
-sdlog2 start -r 20 -a -b 16
+sdlog2 start -r 50 -a -b 16
  
 #
 # Start system state
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 5e80ddc2f..26b561571 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -36,8 +36,5 @@ fi
 #
 if sensors start
 then
-	#
-	# Check sensors - run AFTER 'sensors start'
-	#
 	preflight_check &
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 5b1bd272e..abeed0ca3 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -16,12 +16,26 @@ fi
 sleep 2
 mavlink start -b 230400 -d /dev/ttyACM0
 
+# Stop commander
+if commander stop
+then
+	echo "Commander stopped"
+fi
+sleep 1
+
 # Start the commander
 if commander start
 then
 	echo "Commander started"
 fi
 
+# Stop px4io
+if px4io stop
+then
+	echo "PX4IO stopped"
+fi
+sleep 1
+
 # Start px4io if present
 if px4io start
 then
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index fde201351..42a0c264c 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -780,7 +780,7 @@ start()
 	int fd;
 
 	if (g_dev != nullptr)
-		errx(1, "already started");
+		errx(0, "already started");
 
 	/* create the driver */
 	g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index a95b62468..117583faf 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1370,7 +1370,7 @@ start()
 	int fd, fd_mag;
 
 	if (g_dev != nullptr)
-		errx(1, "already started");
+		errx(0, "already started");
 
 	/* create the driver */
 	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
-- 
cgit v1.2.3


From 408eaf0ad1c8aa87c74f83281de3a7c25fc4b4e6 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 18 Aug 2013 09:22:40 +0200
Subject: Add ioctl to find out if mag is external or onboard

---
 src/drivers/drv_mag.h           |  3 +++
 src/drivers/hmc5883/hmc5883.cpp | 24 +++++++++++++++++++-----
 src/drivers/lsm303d/lsm303d.cpp | 15 ++++++++++++---
 3 files changed, 34 insertions(+), 8 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 9aab995a1..e95034e8e 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -111,4 +111,7 @@ ORB_DECLARE(sensor_mag);
 /** perform self test and report status */
 #define MAGIOCSELFTEST		_MAGIOC(7)
 
+/** determine if mag is external or onboard */
+#define MAGIOCGEXTERNAL		_MAGIOC(8)
+
 #endif /* _DRV_MAG_H */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 039b496f4..9e9c067d5 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -167,6 +167,8 @@ private:
 	bool			_sensor_ok;		/**< sensor was found and reports ok */
 	bool			_calibrated;		/**< the calibration is valid */
 
+	int			_bus;			/**< the bus the device is connected to */
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -326,7 +328,8 @@ HMC5883::HMC5883(int bus) :
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
 	_sensor_ok(false),
-	_calibrated(false)
+	_calibrated(false),
+	_bus(bus)
 {
 	// enable debug() calls
 	_debug_enabled = false;
@@ -665,6 +668,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCSELFTEST:
 		return check_calibration();
 
+	case MAGIOCGEXTERNAL:
+		if (_bus == PX4_I2C_BUS_EXPANSION)
+			return 1;
+		else
+			return 0;
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -851,12 +860,12 @@ HMC5883::collect()
 		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 	} else {
 #endif
-		/* XXX axis assignment of external sensor is yet unknown */
-		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		/* the standard external mag seems to be rolled 180deg, therefore y and z inverted */
+		_reports[_next_report].x = ((report.x * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		_reports[_next_report].y = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
-		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+		_reports[_next_report].z = ((((report.z == -32768) ? 32767 : -report.z) * _range_scale) - _scale.z_offset) * _scale.z_scale;
 #ifdef PX4_I2C_BUS_ONBOARD
 	}
 #endif
@@ -1293,6 +1302,11 @@ test()
 	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
 	warnx("time:        %lld", report.timestamp);
 
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("device active: %s", ret ? "external" : "onboard");
+
 	/* set the queue depth to 10 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
 		errx(1, "failed to set queue depth");
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 117583faf..3e6ce64b8 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -869,6 +869,10 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 //		return self_test();
 		return -EINVAL;
 
+	case MAGIOCGEXTERNAL:
+		/* no external mag board yet */
+		return 0;
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -1422,7 +1426,7 @@ test()
 	int fd_accel = -1;
 	struct accel_report accel_report;
 	ssize_t sz;
-	int filter_bandwidth;
+	int ret;
 
 	/* get the driver */
 	fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
@@ -1445,10 +1449,10 @@ test()
 	warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
 
 	warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
-	if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
+	if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
 		warnx("accel antialias filter bandwidth: fail");
 	else
-		warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth);
+		warnx("accel antialias filter bandwidth: %d Hz", ret);
 
 	int fd_mag = -1;
 	struct mag_report m_report;
@@ -1459,6 +1463,11 @@ test()
 	if (fd_mag < 0)
 		err(1, "%s open failed", MAG_DEVICE_PATH);
 
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("device active: %s", ret ? "external" : "onboard");
+
 	/* do a simple demand read */
 	sz = read(fd_mag, &m_report, sizeof(m_report));
 
-- 
cgit v1.2.3


From f5c92314f16fde650ee6f2f4fa20b7c2680a4b00 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 20 Aug 2013 20:02:06 +0200
Subject: Improved LSM303D driver, plus some fixes to the HMC5883

---
 src/drivers/drv_mag.h           |  25 ++-
 src/drivers/hmc5883/hmc5883.cpp |   5 +
 src/drivers/lsm303d/lsm303d.cpp | 482 +++++++++++++++++++++++-----------------
 src/modules/sensors/sensors.cpp |  25 ++-
 src/systemcmds/config/config.c  |  71 ++++--
 5 files changed, 379 insertions(+), 229 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index e95034e8e..3de5625fd 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -90,28 +90,37 @@ ORB_DECLARE(sensor_mag);
 /** set the mag internal sample rate to at least (arg) Hz */
 #define MAGIOCSSAMPLERATE	_MAGIOC(0)
 
+/** return the mag internal sample rate in Hz */
+#define MAGIOCGSAMPLERATE	_MAGIOC(1)
+
 /** set the mag internal lowpass filter to no lower than (arg) Hz */
-#define MAGIOCSLOWPASS		_MAGIOC(1)
+#define MAGIOCSLOWPASS		_MAGIOC(2)
+
+/** return the mag internal lowpass filter in Hz */
+#define MAGIOCGLOWPASS		_MAGIOC(3)
 
 /** set the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCSSCALE		_MAGIOC(2)
+#define MAGIOCSSCALE		_MAGIOC(4)
 
 /** copy the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCGSCALE		_MAGIOC(3)
+#define MAGIOCGSCALE		_MAGIOC(5)
 
 /** set the measurement range to handle (at least) arg Gauss */
-#define MAGIOCSRANGE		_MAGIOC(4)
+#define MAGIOCSRANGE		_MAGIOC(6)
+
+/** return the current mag measurement range in Gauss */
+#define MAGIOCGRANGE		_MAGIOC(7)
 
 /** perform self-calibration, update scale factors to canonical units */
-#define MAGIOCCALIBRATE		_MAGIOC(5)
+#define MAGIOCCALIBRATE		_MAGIOC(8)
 
 /** excite strap */
-#define MAGIOCEXSTRAP		_MAGIOC(6)
+#define MAGIOCEXSTRAP		_MAGIOC(9)
 
 /** perform self test and report status */
-#define MAGIOCSELFTEST		_MAGIOC(7)
+#define MAGIOCSELFTEST		_MAGIOC(10)
 
 /** determine if mag is external or onboard */
-#define MAGIOCGEXTERNAL		_MAGIOC(8)
+#define MAGIOCGEXTERNAL		_MAGIOC(11)
 
 #endif /* _DRV_MAG_H */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 44304fc22..1afc16a9a 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -637,13 +637,18 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return -EINVAL;
 
 	case MAGIOCSSAMPLERATE:
+	case MAGIOCGSAMPLERATE:
 		/* not supported, always 1 sample per poll */
 		return -EINVAL;
 
 	case MAGIOCSRANGE:
 		return set_range(arg);
 
+	case MAGIOCGRANGE:
+		return _range_ga;
+
 	case MAGIOCSLOWPASS:
+	case MAGIOCGLOWPASS:
 		/* not supported, no internal filtering */
 		return -EINVAL;
 
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 3e6ce64b8..8d6dc0672 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -180,56 +180,62 @@ public:
 	LSM303D(int bus, const char* path, spi_dev_e device);
 	virtual ~LSM303D();
 
-	virtual int			init();
+	virtual int		init();
 
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
 
 	/**
 	 * Diagnostics - print some basic information about the driver.
 	 */
-	void				print_info();
+	void			print_info();
 
 protected:
-	virtual int			probe();
+	virtual int		probe();
 
 	friend class 		LSM303D_mag;
 
 	virtual ssize_t		mag_read(struct file *filp, char *buffer, size_t buflen);
-	virtual int			mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int		mag_ioctl(struct file *filp, int cmd, unsigned long arg);
 
 private:
 
-	LSM303D_mag			*_mag;
+	LSM303D_mag		*_mag;
 
 	struct hrt_call		_accel_call;
 	struct hrt_call		_mag_call;
 
-	unsigned			_call_accel_interval;
-	unsigned			_call_mag_interval;
+	unsigned		_call_accel_interval;
+	unsigned		_call_mag_interval;
 
-	unsigned			_num_accel_reports;
+	unsigned		_num_accel_reports;
 	volatile unsigned	_next_accel_report;
 	volatile unsigned	_oldest_accel_report;
 	struct accel_report	*_accel_reports;
 
-	struct accel_scale	_accel_scale;
-	float				_accel_range_scale;
-	float				_accel_range_m_s2;
-	orb_advert_t		_accel_topic;
-
-	unsigned			_current_samplerate;
-
-	unsigned			_num_mag_reports;
+	unsigned		_num_mag_reports;
 	volatile unsigned	_next_mag_report;
 	volatile unsigned	_oldest_mag_report;
 	struct mag_report	*_mag_reports;
 
+	struct accel_scale	_accel_scale;
+	unsigned		_accel_range_g;
+	float			_accel_range_m_s2;
+	float			_accel_range_scale;
+	unsigned		_accel_samplerate;
+	unsigned		_accel_filter_bandwith;
+
 	struct mag_scale	_mag_scale;
-	float				_mag_range_scale;
-	float				_mag_range_ga;
+	unsigned		_mag_range_ga;
+	float			_mag_range_scale;
+	unsigned		_mag_samplerate;
+
+	orb_advert_t		_accel_topic;
 	orb_advert_t		_mag_topic;
 
+	unsigned		_accel_read;
+	unsigned		_mag_read;
+
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
 
@@ -240,12 +246,19 @@ private:
 	/**
 	 * Start automatic measurement.
 	 */
-	void				start();
+	void			start();
 
 	/**
 	 * Stop automatic measurement.
 	 */
-	void				stop();
+	void			stop();
+
+	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	void			reset();
 
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
@@ -256,24 +269,38 @@ private:
 	 *
 	 * @param arg		Instance pointer for the driver that is polling.
 	 */
-	static void			measure_trampoline(void *arg);
+	static void		measure_trampoline(void *arg);
 
 	/**
 	 * Static trampoline for the mag because it runs at a lower rate
 	 *
 	 * @param arg		Instance pointer for the driver that is polling.
 	 */
-	static void			mag_measure_trampoline(void *arg);
+	static void		mag_measure_trampoline(void *arg);
 
 	/**
 	 * Fetch accel measurements from the sensor and update the report ring.
 	 */
-	void				measure();
+	void			measure();
 
 	/**
 	 * Fetch mag measurements from the sensor and update the report ring.
 	 */
-	void				mag_measure();
+	void			mag_measure();
+
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			accel_self_test();
+
+	/**
+	 * Mag self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int			mag_self_test();
 
 	/**
 	 * Read a register from the LSM303D
@@ -281,7 +308,7 @@ private:
 	 * @param		The register to read.
 	 * @return		The value that was read.
 	 */
-	uint8_t				read_reg(unsigned reg);
+	uint8_t			read_reg(unsigned reg);
 
 	/**
 	 * Write a register in the LSM303D
@@ -289,7 +316,7 @@ private:
 	 * @param reg		The register to write.
 	 * @param value		The new value to write.
 	 */
-	void				write_reg(unsigned reg, uint8_t value);
+	void			write_reg(unsigned reg, uint8_t value);
 
 	/**
 	 * Modify a register in the LSM303D
@@ -300,7 +327,7 @@ private:
 	 * @param clearbits	Bits in the register to clear.
 	 * @param setbits	Bits in the register to set.
 	 */
-	void				modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
 
 	/**
 	 * Set the LSM303D accel measurement range.
@@ -309,7 +336,7 @@ private:
 	 *			Zero selects the maximum supported range.
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int					set_range(unsigned max_g);
+	int			accel_set_range(unsigned max_g);
 
 	/**
 	 * Set the LSM303D mag measurement range.
@@ -318,7 +345,7 @@ private:
 	 *			Zero selects the maximum supported range.
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int					mag_set_range(unsigned max_g);
+	int			mag_set_range(unsigned max_g);
 
 	/**
 	 * Set the LSM303D accel anti-alias filter.
@@ -327,15 +354,7 @@ private:
 	 * 			Zero selects the highest bandwidth
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int					set_antialias_filter_bandwidth(unsigned bandwith);
-
-	/**
-	 * Get the LSM303D accel anti-alias filter.
-	 *
-	 * @param bandwidth The anti-alias filter bandwidth in Hz
-	 * @return		OK if the value was read and supported, ERROR otherwise.
-	 */
-	int					get_antialias_filter_bandwidth(unsigned &bandwidth);
+	int			accel_set_antialias_filter_bandwidth(unsigned bandwith);
 
 	/**
 	 * Set the LSM303D internal accel sampling frequency.
@@ -345,7 +364,7 @@ private:
 	 *			Zero selects the maximum rate supported.
 	 * @return		OK if the value can be supported.
 	 */
-	int					set_samplerate(unsigned frequency);
+	int			accel_set_samplerate(unsigned frequency);
 
 	/**
 	 * Set the LSM303D internal mag sampling frequency.
@@ -355,7 +374,7 @@ private:
 	 *			Zero selects the maximum rate supported.
 	 * @return		OK if the value can be supported.
 	 */
-	int					mag_set_samplerate(unsigned frequency);
+	int			mag_set_samplerate(unsigned frequency);
 };
 
 /**
@@ -396,16 +415,22 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_next_accel_report(0),
 	_oldest_accel_report(0),
 	_accel_reports(nullptr),
-	_accel_range_scale(0.0f),
-	_accel_range_m_s2(0.0f),
-	_accel_topic(-1),
-	_current_samplerate(0),
 	_num_mag_reports(0),
 	_next_mag_report(0),
 	_oldest_mag_report(0),
 	_mag_reports(nullptr),
+	_accel_range_g(8),
+	_accel_range_m_s2(0.0f),
+	_accel_range_scale(0.0f),
+	_accel_samplerate(800),
+	_accel_filter_bandwith(50),
+	_mag_range_ga(2),
 	_mag_range_scale(0.0f),
-	_mag_range_ga(0.0f),
+	_mag_samplerate(100),
+	_accel_topic(-1),
+	_mag_topic(-1),
+	_accel_read(0),
+	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
 	_accel_filter_x(800, 30),
@@ -416,18 +441,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_debug_enabled = true;
 
 	// default scale factors
-	_accel_scale.x_offset = 0;
+	_accel_scale.x_offset = 0.0f;
 	_accel_scale.x_scale  = 1.0f;
-	_accel_scale.y_offset = 0;
+	_accel_scale.y_offset = 0.0f;
 	_accel_scale.y_scale  = 1.0f;
-	_accel_scale.z_offset = 0;
+	_accel_scale.z_offset = 0.0f;
 	_accel_scale.z_scale  = 1.0f;
 
-	_mag_scale.x_offset = 0;
+	_mag_scale.x_offset = 0.0f;
 	_mag_scale.x_scale = 1.0f;
-	_mag_scale.y_offset = 0;
+	_mag_scale.y_offset = 0.0f;
 	_mag_scale.y_scale = 1.0f;
-	_mag_scale.z_offset = 0;
+	_mag_scale.z_offset = 0.0f;
 	_mag_scale.z_scale = 1.0f;
 }
 
@@ -478,27 +503,12 @@ LSM303D::init()
 	if (_mag_reports == nullptr)
 		goto out;
 
+	reset();
+
 	/* advertise mag topic */
 	memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
 	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
 
-	/* enable accel, XXX do this with an ioctl? */
-	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
-
-	/* enable mag, XXX do this with an ioctl? */
-	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
-	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
-
-	/* XXX should we enable FIFO? */
-
-	set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */
-	set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
-	set_samplerate(400); /* max sample rate */
-
-	mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
-	mag_set_samplerate(100);
-
-	/* XXX test this when another mag is used */
 	/* do CDev init for the mag device node, keep it optional */
 	mag_ret = _mag->init();
 
@@ -511,6 +521,24 @@ out:
 	return ret;
 }
 
+void
+LSM303D::reset()
+{
+	/* enable accel*/
+	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
+
+	/* enable mag */
+	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+
+	accel_set_range(_accel_range_g);
+	accel_set_samplerate(_accel_samplerate);
+	accel_set_antialias_filter_bandwidth(_accel_filter_bandwith);
+
+	mag_set_range(_mag_range_ga);
+	mag_set_samplerate(_mag_samplerate);
+}
+
 int
 LSM303D::probe()
 {
@@ -612,64 +640,60 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 
 	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
+		switch (arg) {
 
-				/* switching to manual polling */
+			/* switching to manual polling */
 			case SENSOR_POLLRATE_MANUAL:
 				stop();
 				_call_accel_interval = 0;
 				return OK;
 
-				/* external signalling not supported */
+			/* external signalling not supported */
 			case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
+			/* zero would be bad */
 			case 0:
 				return -EINVAL;
 
-				/* set default/max polling rate */
+			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
 
 			case SENSOR_POLLRATE_DEFAULT:
-				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				/* XXX for vibration tests with 800 Hz */
+				/* Use 800Hz as it is filtered in the driver as well*/
 				return ioctl(filp, SENSORIOCSPOLLRATE, 800);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_call_accel_interval == 0);
+				/* do we need to start internal polling? */
+				bool want_start = (_call_accel_interval == 0);
 
-					/* convert hz to hrt interval via microseconds */
-					unsigned ticks = 1000000 / arg;
+				/* convert hz to hrt interval via microseconds */
+				unsigned ticks = 1000000 / arg;
 
-					/* check against maximum sane rate */
-					if (ticks < 1000)
-						return -EINVAL;
+				/* check against maximum sane rate */
+				if (ticks < 500)
+					return -EINVAL;
 
-					/* adjust sample rate of sensor */
-					set_samplerate(arg);
+				/* adjust filters */
+				float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
 
-                                        // adjust filters
-                                        float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
-                                        float sample_rate = 1.0e6f/ticks;
-                                        _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                                        _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                                        _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+				_accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz);
+				_accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz);
+				_accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz);
 
-					/* update interval for next measurement */
-					/* XXX this is a bit shady, but no other way to adjust... */
-					_accel_call.period = _call_accel_interval = ticks;
+				/* update interval for next measurement */
+				/* XXX this is a bit shady, but no other way to adjust... */
+				_accel_call.period = _call_accel_interval = ticks;
 
-					/* if we need to start the poll state machine, do it */
-					if (want_start)
-						start();
+				/* if we need to start the poll state machine, do it */
+				if (want_start)
+					start();
 
-					return OK;
-				}
+				return OK;
 			}
 		}
+	}
 
 	case SENSORIOCGPOLLRATE:
 		if (_call_accel_interval == 0)
@@ -678,66 +702,78 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_accel_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
-			arg++;
+		/* account for sentinel in the ring */
+		arg++;
 
-			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 2) || (arg > 100))
-				return -EINVAL;
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 2) || (arg > 100))
+			return -EINVAL;
 
-			/* allocate new buffer */
-			struct accel_report *buf = new struct accel_report[arg];
+		/* allocate new buffer */
+		struct accel_report *buf = new struct accel_report[arg];
 
-			if (nullptr == buf)
-				return -ENOMEM;
+		if (nullptr == buf)
+			return -ENOMEM;
 
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _accel_reports;
-			_num_accel_reports = arg;
-			_accel_reports = buf;
-			start();
+		/* reset the measurement state machine with the new buffer, free the old */
+		stop();
+		delete[] _accel_reports;
+		_num_accel_reports = arg;
+		_accel_reports = buf;
+		start();
 
-			return OK;
-		}
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _num_accel_reports - 1;
 
 	case SENSORIOCRESET:
-		/* XXX implement */
-		return -EINVAL;
+		reset();
+		return OK;
+
+	case ACCELIOCSSAMPLERATE:
+		return accel_set_samplerate(arg);
+
+	case ACCELIOCGSAMPLERATE:
+		return _accel_samplerate;
 
 	case ACCELIOCSLOWPASS: {
-                float cutoff_freq_hz = arg;
-                float sample_rate = 1.0e6f / _call_accel_interval;
-                _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                return OK;
-        }
+		_accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
+		_accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
+		_accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
+		return OK;
+	}
 
 	case ACCELIOCGLOWPASS:
-                return _accel_filter_x.get_cutoff_freq();
-
-	case ACCELIOCSSCALE:
-		{
-			/* copy scale, but only if off by a few percent */
-			struct accel_scale *s = (struct accel_scale *) arg;
-			float sum = s->x_scale + s->y_scale + s->z_scale;
-			if (sum > 2.0f && sum < 4.0f) {
-				memcpy(&_accel_scale, s, sizeof(_accel_scale));
-				return OK;
-			} else {
-				return -EINVAL;
-			}
+		return _accel_filter_x.get_cutoff_freq();
+
+	case ACCELIOCSSCALE: {
+		/* copy scale, but only if off by a few percent */
+		struct accel_scale *s = (struct accel_scale *) arg;
+		float sum = s->x_scale + s->y_scale + s->z_scale;
+		if (sum > 2.0f && sum < 4.0f) {
+			memcpy(&_accel_scale, s, sizeof(_accel_scale));
+			return OK;
+		} else {
+			return -EINVAL;
 		}
+	}
+
+	case ACCELIOCSRANGE:
+		return accel_set_range(arg);
+
+	case ACCELIOCGRANGE:
+		return _accel_range_g;
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
 		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
 		return OK;
 
+	case ACCELIOCSELFTEST:
+		return accel_self_test();
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -768,7 +804,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* 50 Hz is max for mag */
+				/* 100 Hz is max for mag */
 				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
 
 			/* adjust to a legal polling interval in Hz */
@@ -783,9 +819,6 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 					if (ticks < 1000)
 						return -EINVAL;
 
-					/* adjust sample rate of sensor */
-					mag_set_samplerate(arg);
-
 					/* update interval for next measurement */
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_mag_call.period = _call_mag_interval = ticks;
@@ -833,17 +866,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _num_mag_reports - 1;
 
 	case SENSORIOCRESET:
-		return ioctl(filp, cmd, arg);
+		reset();
+		return OK;
 
 	case MAGIOCSSAMPLERATE:
-//	case MAGIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+		return mag_set_samplerate(arg);
+
+	case MAGIOCGSAMPLERATE:
+		return _mag_samplerate;
 
 	case MAGIOCSLOWPASS:
-//	case MAGIOCGLOWPASS:
-		/* XXX not implemented */
-//		_set_dlpf_filter((uint16_t)arg);
+	case MAGIOCGLOWPASS:
+		/* not supported, no internal filtering */
 		return -EINVAL;
 
 	case MAGIOCSSCALE:
@@ -857,17 +891,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case MAGIOCSRANGE:
-//	case MAGIOCGRANGE:
-		/* XXX not implemented */
-		// XXX change these two values on set:
-		// _mag_range_scale = xx
-		// _mag_range_ga = xx
-		return -EINVAL;
+		return mag_set_range(arg);
+
+	case MAGIOCGRANGE:
+		return _mag_range_ga;
 
 	case MAGIOCSELFTEST:
-		/* XXX not implemented */
-//		return self_test();
-		return -EINVAL;
+		return mag_self_test();
 
 	case MAGIOCGEXTERNAL:
 		/* no external mag board yet */
@@ -879,6 +909,53 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 	}
 }
 
+int
+LSM303D::accel_self_test()
+{
+	if (_accel_read == 0)
+		return 1;
+
+	/* inspect accel offsets */
+	if (fabsf(_accel_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+int
+LSM303D::mag_self_test()
+{
+	if (_mag_read == 0)
+		return 1;
+
+	/**
+	 * inspect mag offsets
+	 * don't check mag scale because it seems this is calibrated on chip
+	 */
+	if (fabsf(_mag_scale.x_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.y_offset) < 0.000001f)
+		return 1;
+
+	if (fabsf(_mag_scale.z_offset) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
 uint8_t
 LSM303D::read_reg(unsigned reg)
 {
@@ -914,38 +991,37 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 }
 
 int
-LSM303D::set_range(unsigned max_g)
+LSM303D::accel_set_range(unsigned max_g)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
-	float new_range_g = 0.0f;
 	float new_scale_g_digit = 0.0f;
 
 	if (max_g == 0)
 		max_g = 16;
 
 	if (max_g <= 2) {
-		new_range_g = 2.0f;
+		_accel_range_g = 2;
 		setbits |= REG2_FULL_SCALE_2G_A;
 		new_scale_g_digit = 0.061e-3f;
 
 	} else if (max_g <= 4) {
-		new_range_g = 4.0f;
+		_accel_range_g = 4;
 		setbits |= REG2_FULL_SCALE_4G_A;
 		new_scale_g_digit = 0.122e-3f;
 
 	} else if (max_g <= 6) {
-		new_range_g = 6.0f;
+		_accel_range_g = 6;
 		setbits |= REG2_FULL_SCALE_6G_A;
 		new_scale_g_digit = 0.183e-3f;
 
 	} else if (max_g <= 8) {
-		new_range_g = 8.0f;
+		_accel_range_g = 8;
 		setbits |= REG2_FULL_SCALE_8G_A;
 		new_scale_g_digit = 0.244e-3f;
 
 	} else if (max_g <= 16) {
-		new_range_g = 16.0f;
+		_accel_range_g = 16;
 		setbits |= REG2_FULL_SCALE_16G_A;
 		new_scale_g_digit = 0.732e-3f;
 
@@ -953,7 +1029,7 @@ LSM303D::set_range(unsigned max_g)
 		return -EINVAL;
 	}
 
-	_accel_range_m_s2 = new_range_g * 9.80665f;
+	_accel_range_m_s2 = _accel_range_g * 9.80665f;
 	_accel_range_scale = new_scale_g_digit * 9.80665f;
 
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
@@ -966,29 +1042,28 @@ LSM303D::mag_set_range(unsigned max_ga)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
-	float new_range = 0.0f;
 	float new_scale_ga_digit = 0.0f;
 
 	if (max_ga == 0)
 		max_ga = 12;
 
 	if (max_ga <= 2) {
-		new_range = 2.0f;
+		_mag_range_ga = 2;
 		setbits |= REG6_FULL_SCALE_2GA_M;
 		new_scale_ga_digit = 0.080e-3f;
 
 	} else if (max_ga <= 4) {
-		new_range = 4.0f;
+		_mag_range_ga = 4;
 		setbits |= REG6_FULL_SCALE_4GA_M;
 		new_scale_ga_digit = 0.160e-3f;
 
 	} else if (max_ga <= 8) {
-		new_range = 8.0f;
+		_mag_range_ga = 8;
 		setbits |= REG6_FULL_SCALE_8GA_M;
 		new_scale_ga_digit = 0.320e-3f;
 
 	} else if (max_ga <= 12) {
-		new_range = 12.0f;
+		_mag_range_ga = 12;
 		setbits |= REG6_FULL_SCALE_12GA_M;
 		new_scale_ga_digit = 0.479e-3f;
 
@@ -996,7 +1071,6 @@ LSM303D::mag_set_range(unsigned max_ga)
 		return -EINVAL;
 	}
 
-	_mag_range_ga = new_range;
 	_mag_range_scale = new_scale_ga_digit;
 
 	modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
@@ -1005,7 +1079,7 @@ LSM303D::mag_set_range(unsigned max_ga)
 }
 
 int
-LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
+LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
@@ -1015,15 +1089,19 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
 
 	if (bandwidth <= 50) {
 		setbits |= REG2_AA_FILTER_BW_50HZ_A;
+		_accel_filter_bandwith = 50;
 
 	} else if (bandwidth <= 194) {
 		setbits |= REG2_AA_FILTER_BW_194HZ_A;
+		_accel_filter_bandwith = 194;
 
 	} else if (bandwidth <= 362) {
 		setbits |= REG2_AA_FILTER_BW_362HZ_A;
+		_accel_filter_bandwith = 362;
 
 	} else if (bandwidth <= 773) {
 		setbits |= REG2_AA_FILTER_BW_773HZ_A;
+		_accel_filter_bandwith = 773;
 
 	} else {
 		return -EINVAL;
@@ -1035,26 +1113,7 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
 }
 
 int
-LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth)
-{
-	uint8_t readbits = read_reg(ADDR_CTRL_REG2);
-
-	if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A)
-		bandwidth = 50;
-	else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A)
-		bandwidth = 194;
-	else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A)
-		bandwidth = 362;
-	else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A)
-		bandwidth = 773;
-	else
-		return ERROR;
-
-	return OK;
-}
-
-int
-LSM303D::set_samplerate(unsigned frequency)
+LSM303D::accel_set_samplerate(unsigned frequency)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG1_RATE_BITS_A;
@@ -1064,23 +1123,23 @@ LSM303D::set_samplerate(unsigned frequency)
 
 	if (frequency <= 100) {
 		setbits |= REG1_RATE_100HZ_A;
-		_current_samplerate = 100;
+		_accel_samplerate = 100;
 
 	} else if (frequency <= 200) {
 		setbits |= REG1_RATE_200HZ_A;
-		_current_samplerate = 200;
+		_accel_samplerate = 200;
 
 	} else if (frequency <= 400) {
 		setbits |= REG1_RATE_400HZ_A;
-		_current_samplerate = 400;
+		_accel_samplerate = 400;
 
 	} else if (frequency <= 800) {
 		setbits |= REG1_RATE_800HZ_A;
-		_current_samplerate = 800;
+		_accel_samplerate = 800;
 
 	} else if (frequency <= 1600) {
 		setbits |= REG1_RATE_1600HZ_A;
-		_current_samplerate = 1600;
+		_accel_samplerate = 1600;
 
 	} else {
 		return -EINVAL;
@@ -1102,13 +1161,15 @@ LSM303D::mag_set_samplerate(unsigned frequency)
 
 	if (frequency <= 25) {
 		setbits |= REG5_RATE_25HZ_M;
+		_mag_samplerate = 25;
 
 	} else if (frequency <= 50) {
 		setbits |= REG5_RATE_50HZ_M;
+		_mag_samplerate = 50;
 
 	} else if (frequency <= 100) {
 		setbits |= REG5_RATE_100HZ_M;
-
+		_mag_samplerate = 100;
 
 	} else {
 		return -EINVAL;
@@ -1229,6 +1290,8 @@ LSM303D::measure()
 	/* publish for subscribers */
 	orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
 
+	_accel_read++;
+
 	/* stop the perf counter */
 	perf_end(_accel_sample_perf);
 }
@@ -1281,7 +1344,7 @@ LSM303D::mag_measure()
 	mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
 	mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
 	mag_report->scaling = _mag_range_scale;
-	mag_report->range_ga = _mag_range_ga;
+	mag_report->range_ga = (float)_mag_range_ga;
 
 	/* post a report to the ring - note, not locked */
 	INCREMENT(_next_mag_report, _num_mag_reports);
@@ -1297,6 +1360,8 @@ LSM303D::mag_measure()
 	/* publish for subscribers */
 	orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
 
+	_mag_read++;
+
 	/* stop the perf counter */
 	perf_end(_mag_sample_perf);
 }
@@ -1304,6 +1369,8 @@ LSM303D::mag_measure()
 void
 LSM303D::print_info()
 {
+	printf("accel reads:          %u\n", _accel_read);
+	printf("mag reads:            %u\n", _mag_read);
 	perf_print_counter(_accel_sample_perf);
 	printf("report queue:   %u (%u/%u @ %p)\n",
 	       _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
@@ -1466,7 +1533,7 @@ test()
 	/* check if mag is onboard or external */
 	if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
 		errx(1, "failed to get if mag is onboard or external");
-	warnx("device active: %s", ret ? "external" : "onboard");
+	warnx("mag device active: %s", ret ? "external" : "onboard");
 
 	/* do a simple demand read */
 	sz = read(fd_mag, &m_report, sizeof(m_report));
@@ -1484,7 +1551,7 @@ test()
 
 	/* XXX add poll-rate tests here too */
 
-//	reset();
+	reset();
 	errx(0, "PASS");
 }
 
@@ -1503,7 +1570,16 @@ reset()
 		err(1, "driver reset failed");
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
+		err(1, "accel driver poll rate reset failed");
+
+	int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd_mag < 0) {
+		warnx("could not open to mag " MAG_DEVICE_PATH);
+	} else {
+		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+			err(1, "mag driver poll rate reset failed");
+	}
 
 	exit(0);
 }
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 198da9f0a..7ea1ae0f3 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -936,6 +936,7 @@ void
 Sensors::mag_init()
 {
 	int	fd;
+	int	ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -944,13 +945,27 @@ Sensors::mag_init()
 		errx(1, "FATAL: no magnetometer found");
 	}
 
-	/* set the mag internal poll rate to at least 150Hz */
-	ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	/* try different mag sampling rates */
 
-	/* set the driver to poll at 150Hz */
-	ioctl(fd, SENSORIOCSPOLLRATE, 150);
+#if 0
+	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	if (ret == OK) {
+		/* set the pollrate accordingly */
+		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+	} else {
+		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+		/* if the slower sampling rate still fails, something is wrong */
+		if (ret == OK) {
+			/* set the driver to poll also at the slower rate */
+			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+		} else {
+			errx(1, "FATAL: mag sampling rate could not be set");
+		}
+	}
+#endif
+	
 
-	int ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
 	if (ret < 0)
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
 	else if (ret == 1)
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 766598ddd..262a52d20 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -90,6 +90,7 @@ static void
 do_gyro(int argc, char *argv[])
 {
 	int	fd;
+	int	ret;
 
 	fd = open(GYRO_DEVICE_PATH, 0);
 
@@ -102,20 +103,29 @@ do_gyro(int argc, char *argv[])
 		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
 			/* set the gyro internal sampling rate up to at least i Hz */
-			ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"sampling rate could not be set");
 
 		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
 
 			/* set the driver to poll at i Hz */
-			ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"pollrate could not be set");
 
 		} else if (argc == 2 && !strcmp(argv[0], "range")) {
 
 			/* set the range to i dps */
-			ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"range could not be set");
 
 		} else if(argc == 1 && !strcmp(argv[0], "check")) {
-			int ret = ioctl(fd, GYROIOCSELFTEST, 0);
+			ret = ioctl(fd, GYROIOCSELFTEST, 0);
 
 			if (ret) {
 				warnx("gyro self test FAILED! Check calibration:");
@@ -147,6 +157,7 @@ static void
 do_mag(int argc, char *argv[])
 {
 	int fd;
+	int ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -156,8 +167,32 @@ do_mag(int argc, char *argv[])
 
 	} else {
 
-		if(argc == 1 && !strcmp(argv[0], "check")) {
-			int ret = ioctl(fd, MAGIOCSELFTEST, 0);
+		if (argc == 2 && !strcmp(argv[0], "sampling")) {
+
+			/* set the mag internal sampling rate up to at least i Hz */
+			ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"sampling rate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
+
+			/* set the driver to poll at i Hz */
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"pollrate could not be set");
+
+		} else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+			/* set the range to i G */
+			ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"range could not be set");
+
+		} else if(argc == 1 && !strcmp(argv[0], "check")) {
+			ret = ioctl(fd, MAGIOCSELFTEST, 0);
 
 			if (ret) {
 				warnx("mag self test FAILED! Check calibration:");
@@ -173,9 +208,9 @@ do_mag(int argc, char *argv[])
 			errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t");
 		}
 
-		int srate = -1; //ioctl(fd, MAGIOCGSAMPLERATE, 0);
+		int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
-		int range = -1; //ioctl(fd, MAGIOCGRANGE, 0);
+		int range = ioctl(fd, MAGIOCGRANGE, 0);
 
 		warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range);
 
@@ -189,6 +224,7 @@ static void
 do_accel(int argc, char *argv[])
 {
 	int	fd;
+	int	ret;
 
 	fd = open(ACCEL_DEVICE_PATH, 0);
 
@@ -201,20 +237,29 @@ do_accel(int argc, char *argv[])
 		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
 			/* set the accel internal sampling rate up to at least i Hz */
-			ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"sampling rate could not be set");
 
 		} else if (argc == 2 && !strcmp(argv[0], "rate")) {
 
 			/* set the driver to poll at i Hz */
-			ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+			ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"pollrate could not be set");
 
 		} else if (argc == 2 && !strcmp(argv[0], "range")) {
 
-			/* set the range to i m/s^2 */
-			ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
+			/* set the range to i G */
+			ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+			if (ret)
+				errx(ret,"range could not be set");
 
 		} else if(argc == 1 && !strcmp(argv[0], "check")) {
-			int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+			ret = ioctl(fd, ACCELIOCSELFTEST, 0);
 
 			if (ret) {
 				warnx("accel self test FAILED! Check calibration:");
-- 
cgit v1.2.3


From 408b29ba618e1c2c7375d6acd488c223d796186f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 21 Aug 2013 08:40:51 +0200
Subject: Don't store m/s^2 and G at the same time

---
 src/drivers/lsm303d/lsm303d.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 8d6dc0672..2b7769992 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -220,7 +220,6 @@ private:
 
 	struct accel_scale	_accel_scale;
 	unsigned		_accel_range_g;
-	float			_accel_range_m_s2;
 	float			_accel_range_scale;
 	unsigned		_accel_samplerate;
 	unsigned		_accel_filter_bandwith;
@@ -420,7 +419,6 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_oldest_mag_report(0),
 	_mag_reports(nullptr),
 	_accel_range_g(8),
-	_accel_range_m_s2(0.0f),
 	_accel_range_scale(0.0f),
 	_accel_samplerate(800),
 	_accel_filter_bandwith(50),
@@ -1029,7 +1027,6 @@ LSM303D::accel_set_range(unsigned max_g)
 		return -EINVAL;
 	}
 
-	_accel_range_m_s2 = _accel_range_g * 9.80665f;
 	_accel_range_scale = new_scale_g_digit * 9.80665f;
 
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
@@ -1275,7 +1272,7 @@ LSM303D::measure()
 	accel_report->z = _accel_filter_z.apply(z_in_new);
 
 	accel_report->scaling = _accel_range_scale;
-	accel_report->range_m_s2 = _accel_range_m_s2;
+	accel_report->range_m_s2 = _accel_range_g * 9.80665f;
 
 	/* post a report to the ring - note, not locked */
 	INCREMENT(_next_accel_report, _num_accel_reports);
-- 
cgit v1.2.3


From fc24037b03b0df3d8d75a98b929c7b37753e0337 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 21 Aug 2013 12:37:06 +0200
Subject: Changed range handling of LSM303D once again, added defines for
 default values

---
 src/drivers/lsm303d/lsm303d.cpp | 63 +++++++++++++++++++++++++----------------
 1 file changed, 39 insertions(+), 24 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 2b7769992..803cd658f 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -54,6 +54,7 @@
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
+#include <systemlib/geo/geo.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
@@ -168,6 +169,14 @@ static const int ERROR = -1;
 #define INT_CTRL_M              0x12
 #define INT_SRC_M               0x13
 
+/* default values for this device */
+#define ACCEL_DEFAULT_RANGE_G 8
+#define ACCEL_DEFAULT_SAMPLERATE 800
+#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
+#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define MAG_DEFAULT_RANGE_GA 2
+#define MAG_DEFAULT_SAMPLERATE 100
 
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
@@ -219,7 +228,7 @@ private:
 	struct mag_report	*_mag_reports;
 
 	struct accel_scale	_accel_scale;
-	unsigned		_accel_range_g;
+	unsigned		_accel_range_m_s2;
 	float			_accel_range_scale;
 	unsigned		_accel_samplerate;
 	unsigned		_accel_filter_bandwith;
@@ -418,22 +427,22 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_next_mag_report(0),
 	_oldest_mag_report(0),
 	_mag_reports(nullptr),
-	_accel_range_g(8),
+	_accel_range_m_s2(0.0f),
 	_accel_range_scale(0.0f),
-	_accel_samplerate(800),
-	_accel_filter_bandwith(50),
-	_mag_range_ga(2),
+	_accel_samplerate(0),
+	_accel_filter_bandwith(0),
+	_mag_range_ga(0.0f),
 	_mag_range_scale(0.0f),
-	_mag_samplerate(100),
+	_mag_samplerate(0),
 	_accel_topic(-1),
 	_mag_topic(-1),
 	_accel_read(0),
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
-	_accel_filter_x(800, 30),
-	_accel_filter_y(800, 30),
-	_accel_filter_z(800, 30)
+	_accel_filter_x(0, 0),
+	_accel_filter_y(0, 0),
+	_accel_filter_z(0, 0)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -529,12 +538,17 @@ LSM303D::reset()
 	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
 	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
 
-	accel_set_range(_accel_range_g);
-	accel_set_samplerate(_accel_samplerate);
-	accel_set_antialias_filter_bandwidth(_accel_filter_bandwith);
+	_accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
+
+	accel_set_range(ACCEL_DEFAULT_RANGE_G);
+	accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE);
+	accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
+
+	mag_set_range(MAG_DEFAULT_RANGE_GA);
+	mag_set_samplerate(MAG_DEFAULT_SAMPLERATE);
 
-	mag_set_range(_mag_range_ga);
-	mag_set_samplerate(_mag_samplerate);
+	_accel_read = 0;
+	_mag_read = 0;
 }
 
 int
@@ -658,8 +672,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
 
 			case SENSOR_POLLRATE_DEFAULT:
-				/* Use 800Hz as it is filtered in the driver as well*/
-				return ioctl(filp, SENSORIOCSPOLLRATE, 800);
+				return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -759,10 +772,12 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 	}
 
 	case ACCELIOCSRANGE:
+		/* arg needs to be in G */
 		return accel_set_range(arg);
 
 	case ACCELIOCGRANGE:
-		return _accel_range_g;
+		/* convert to m/s^2 and return rounded in G */
+		return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
@@ -999,27 +1014,27 @@ LSM303D::accel_set_range(unsigned max_g)
 		max_g = 16;
 
 	if (max_g <= 2) {
-		_accel_range_g = 2;
+		_accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_2G_A;
 		new_scale_g_digit = 0.061e-3f;
 
 	} else if (max_g <= 4) {
-		_accel_range_g = 4;
+		_accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_4G_A;
 		new_scale_g_digit = 0.122e-3f;
 
 	} else if (max_g <= 6) {
-		_accel_range_g = 6;
+		_accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_6G_A;
 		new_scale_g_digit = 0.183e-3f;
 
 	} else if (max_g <= 8) {
-		_accel_range_g = 8;
+		_accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_8G_A;
 		new_scale_g_digit = 0.244e-3f;
 
 	} else if (max_g <= 16) {
-		_accel_range_g = 16;
+		_accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G;
 		setbits |= REG2_FULL_SCALE_16G_A;
 		new_scale_g_digit = 0.732e-3f;
 
@@ -1027,7 +1042,7 @@ LSM303D::accel_set_range(unsigned max_g)
 		return -EINVAL;
 	}
 
-	_accel_range_scale = new_scale_g_digit * 9.80665f;
+	_accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G;
 
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
@@ -1272,7 +1287,7 @@ LSM303D::measure()
 	accel_report->z = _accel_filter_z.apply(z_in_new);
 
 	accel_report->scaling = _accel_range_scale;
-	accel_report->range_m_s2 = _accel_range_g * 9.80665f;
+	accel_report->range_m_s2 = _accel_range_m_s2;
 
 	/* post a report to the ring - note, not locked */
 	INCREMENT(_next_accel_report, _num_accel_reports);
-- 
cgit v1.2.3


From 1ede95d252f5401f3bcf94265c42a060833ca8ca Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 21 Aug 2013 14:21:54 +0200
Subject: L3GD20 and LSM303D reset and range config working properly now

---
 src/drivers/l3gd20/l3gd20.cpp   | 131 ++++++++++++++++++++++++++--------------
 src/drivers/lsm303d/lsm303d.cpp |  92 ++++++++++++++++------------
 2 files changed, 136 insertions(+), 87 deletions(-)

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index de6b753f1..5e0a2119a 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -154,6 +154,10 @@ static const int ERROR = -1;
 #define FIFO_CTRL_STREAM_TO_FIFO_MODE		(3<<5)
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
 
+#define L3GD20_DEFAULT_RATE			760
+#define L3GD20_DEFAULT_RANGE_DPS		2000
+#define L3GD20_DEFAULT_FILTER_FREQ		30
+
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 
 class L3GD20 : public device::SPI
@@ -191,9 +195,10 @@ private:
 	orb_advert_t		_gyro_topic;
 
 	unsigned		_current_rate;
-	unsigned		_current_range;
 	unsigned		_orientation;
 
+	unsigned		_read;
+
 	perf_counter_t		_sample_perf;
 
 	math::LowPassFilter2p	_gyro_filter_x;
@@ -210,6 +215,11 @@ private:
 	 */
 	void			stop();
 
+	/**
+	 * Reset the driver
+	 */
+	void			reset();
+
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
@@ -273,6 +283,14 @@ private:
 	 */
 	int			set_samplerate(unsigned frequency);
 
+	/**
+	 * Set the lowpass filter of the driver
+	 *
+	 * @param samplerate	The current samplerate
+	 * @param frequency	The cutoff frequency for the lowpass filter
+	 */
+	void			set_driver_lowpass_filter(float samplerate, float bandwidth);
+
 	/**
 	 * Self test
 	 *
@@ -296,12 +314,12 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
 	_current_rate(0),
-	_current_range(0),
 	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
-        _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
-        _gyro_filter_x(250, 30),
-        _gyro_filter_y(250, 30),
-        _gyro_filter_z(250, 30)
+	_read(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
+	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -349,22 +367,7 @@ L3GD20::init()
 	memset(&_reports[0], 0, sizeof(_reports[0]));
 	_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
 
-	/* set default configuration */
-	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
-	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
-	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
-	write_reg(ADDR_CTRL_REG4, REG4_BDU);
-	write_reg(ADDR_CTRL_REG5, 0);
-
-	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
-
-        /* disable FIFO. This makes things simpler and ensures we
-         * aren't getting stale data. It means we must run the hrt
-         * callback fast enough to not miss data. */
-	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);	
-
-	set_range(2000);			/* default to 2000dps */
-	set_samplerate(0);			/* max sample rate */
+	reset();
 
 	ret = OK;
 out:
@@ -464,8 +467,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+				return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -483,12 +485,10 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 					/* XXX this is a bit shady, but no other way to adjust... */
 					_call.period = _call_interval = ticks;
 
-                                        // adjust filters
-                                        float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
-                                        float sample_rate = 1.0e6f/ticks;
-                                        _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                                        _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                                        _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+					/* adjust filters */
+					float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
+					float sample_rate = 1.0e6f/ticks;
+					set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
@@ -533,8 +533,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _num_reports - 1;
 
 	case SENSORIOCRESET:
-		/* XXX implement */
-		return -EINVAL;
+		reset();
+		return OK;
 
 	case GYROIOCSSAMPLERATE:
 		return set_samplerate(arg);
@@ -543,16 +543,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _current_rate;
 
 	case GYROIOCSLOWPASS: {
-                float cutoff_freq_hz = arg;
-                float sample_rate = 1.0e6f / _call_interval;
-                _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-                return OK;
-        }
+		float cutoff_freq_hz = arg;
+		float sample_rate = 1.0e6f / _call_interval;
+		set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
+		return OK;
+	}
 
 	case GYROIOCGLOWPASS:
-                return _gyro_filter_x.get_cutoff_freq();
+		return _gyro_filter_x.get_cutoff_freq();
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
@@ -565,10 +564,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	case GYROIOCSRANGE:
+		/* arg should be in dps */
 		return set_range(arg);
 
 	case GYROIOCGRANGE:
-		return _current_range;
+		/* convert to dps and round */
+		return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
 
 	case GYROIOCSELFTEST:
 		return self_test();
@@ -618,22 +619,23 @@ L3GD20::set_range(unsigned max_dps)
 {
 	uint8_t bits = REG4_BDU;
 	float new_range_scale_dps_digit;
+	float new_range;
 
 	if (max_dps == 0) {
 		max_dps = 2000;
 	}
 	if (max_dps <= 250) {
-		_current_range = 250;
+		new_range = 250;
 		bits |= RANGE_250DPS;
 		new_range_scale_dps_digit = 8.75e-3f;
 
 	} else if (max_dps <= 500) {
-		_current_range = 500;
+		new_range = 500;
 		bits |= RANGE_500DPS;
 		new_range_scale_dps_digit = 17.5e-3f;
 
 	} else if (max_dps <= 2000) {
-		_current_range = 2000;
+		new_range = 2000;
 		bits |= RANGE_2000DPS;
 		new_range_scale_dps_digit = 70e-3f;
 
@@ -641,7 +643,7 @@ L3GD20::set_range(unsigned max_dps)
 		return -EINVAL;
 	}
 
-	_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
+	_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
 	_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
 	write_reg(ADDR_CTRL_REG4, bits);
 
@@ -656,7 +658,7 @@ L3GD20::set_samplerate(unsigned frequency)
 	if (frequency == 0)
 		frequency = 760;
 
-        // use limits good for H or non-H models
+	/* use limits good for H or non-H models */
 	if (frequency <= 100) {
 		_current_rate = 95;
 		bits |= RATE_95HZ_LP_25HZ;
@@ -682,6 +684,14 @@ L3GD20::set_samplerate(unsigned frequency)
 	return OK;
 }
 
+void
+L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+}
+
 void
 L3GD20::start()
 {
@@ -701,6 +711,30 @@ L3GD20::stop()
 	hrt_cancel(&_call);
 }
 
+void
+L3GD20::reset()
+{
+	/* set default configuration */
+	write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+	write_reg(ADDR_CTRL_REG2, 0);		/* disable high-pass filters */
+	write_reg(ADDR_CTRL_REG3, 0);		/* no interrupts - we don't use them */
+	write_reg(ADDR_CTRL_REG4, REG4_BDU);
+	write_reg(ADDR_CTRL_REG5, 0);
+
+	write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE);		/* disable wake-on-interrupt */
+
+	/* disable FIFO. This makes things simpler and ensures we
+	 * aren't getting stale data. It means we must run the hrt
+	 * callback fast enough to not miss data. */
+	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+
+	set_samplerate(L3GD20_DEFAULT_RATE);
+	set_range(L3GD20_DEFAULT_RANGE_DPS);
+	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
+
+	_read = 0;
+}
+
 void
 L3GD20::measure_trampoline(void *arg)
 {
@@ -804,6 +838,8 @@ L3GD20::measure()
 	if (_gyro_topic > 0)
 		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
 
+	_read++;
+
 	/* stop the perf counter */
 	perf_end(_sample_perf);
 }
@@ -811,6 +847,7 @@ L3GD20::measure()
 void
 L3GD20::print_info()
 {
+	printf("gyro reads:          %u\n", _read);
 	perf_print_counter(_sample_perf);
 	printf("report queue:   %u (%u/%u @ %p)\n",
 	       _num_reports, _oldest_report, _next_report, _reports);
@@ -949,7 +986,7 @@ reset()
 		err(1, "driver reset failed");
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "driver poll restart failed");
+		err(1, "accel pollrate reset failed");
 
 	exit(0);
 }
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 803cd658f..efe7cf8eb 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -170,13 +170,13 @@ static const int ERROR = -1;
 #define INT_SRC_M               0x13
 
 /* default values for this device */
-#define ACCEL_DEFAULT_RANGE_G 8
-#define ACCEL_DEFAULT_SAMPLERATE 800
-#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
-#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+#define LSM303D_ACCEL_DEFAULT_RANGE_G			8
+#define LSM303D_ACCEL_DEFAULT_RATE			800
+#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ	50
+#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
 
-#define MAG_DEFAULT_RANGE_GA 2
-#define MAG_DEFAULT_SAMPLERATE 100
+#define LSM303D_MAG_DEFAULT_RANGE_GA			2
+#define LSM303D_MAG_DEFAULT_RATE			100
 
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
@@ -231,7 +231,7 @@ private:
 	unsigned		_accel_range_m_s2;
 	float			_accel_range_scale;
 	unsigned		_accel_samplerate;
-	unsigned		_accel_filter_bandwith;
+	unsigned		_accel_onchip_filter_bandwith;
 
 	struct mag_scale	_mag_scale;
 	unsigned		_mag_range_ga;
@@ -356,13 +356,22 @@ private:
 	int			mag_set_range(unsigned max_g);
 
 	/**
-	 * Set the LSM303D accel anti-alias filter.
+	 * Set the LSM303D on-chip anti-alias filter bandwith.
 	 *
 	 * @param bandwidth The anti-alias filter bandwidth in Hz
 	 * 			Zero selects the highest bandwidth
 	 * @return		OK if the value can be supported, -ERANGE otherwise.
 	 */
-	int			accel_set_antialias_filter_bandwidth(unsigned bandwith);
+	int			accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
+
+	/**
+	 * Set the driver lowpass filter bandwidth.
+	 *
+	 * @param bandwidth The anti-alias filter bandwidth in Hz
+	 * 			Zero selects the highest bandwidth
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
 
 	/**
 	 * Set the LSM303D internal accel sampling frequency.
@@ -430,7 +439,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_accel_range_m_s2(0.0f),
 	_accel_range_scale(0.0f),
 	_accel_samplerate(0),
-	_accel_filter_bandwith(0),
+	_accel_onchip_filter_bandwith(0),
 	_mag_range_ga(0.0f),
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
@@ -440,9 +449,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
-	_accel_filter_x(0, 0),
-	_accel_filter_y(0, 0),
-	_accel_filter_z(0, 0)
+	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -538,14 +547,13 @@ LSM303D::reset()
 	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
 	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
 
-	_accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
-
-	accel_set_range(ACCEL_DEFAULT_RANGE_G);
-	accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE);
-	accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
+	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
+	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
+	accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
+	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
 
-	mag_set_range(MAG_DEFAULT_RANGE_GA);
-	mag_set_samplerate(MAG_DEFAULT_SAMPLERATE);
+	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
+	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
 
 	_accel_read = 0;
 	_mag_read = 0;
@@ -672,7 +680,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
 
 			case SENSOR_POLLRATE_DEFAULT:
-				return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE);
+				return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {
@@ -687,11 +695,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 					return -EINVAL;
 
 				/* adjust filters */
-				float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
-
-				_accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz);
-				_accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz);
-				_accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz);
+				accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
 
 				/* update interval for next measurement */
 				/* XXX this is a bit shady, but no other way to adjust... */
@@ -750,10 +754,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _accel_samplerate;
 
 	case ACCELIOCSLOWPASS: {
-		_accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
-		_accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
-		_accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
-		return OK;
+		return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
 	}
 
 	case ACCELIOCGLOWPASS:
@@ -1091,7 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga)
 }
 
 int
-LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth)
+LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
 {
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
@@ -1101,19 +1102,19 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth)
 
 	if (bandwidth <= 50) {
 		setbits |= REG2_AA_FILTER_BW_50HZ_A;
-		_accel_filter_bandwith = 50;
+		_accel_onchip_filter_bandwith = 50;
 
 	} else if (bandwidth <= 194) {
 		setbits |= REG2_AA_FILTER_BW_194HZ_A;
-		_accel_filter_bandwith = 194;
+		_accel_onchip_filter_bandwith = 194;
 
 	} else if (bandwidth <= 362) {
 		setbits |= REG2_AA_FILTER_BW_362HZ_A;
-		_accel_filter_bandwith = 362;
+		_accel_onchip_filter_bandwith = 362;
 
 	} else if (bandwidth <= 773) {
 		setbits |= REG2_AA_FILTER_BW_773HZ_A;
-		_accel_filter_bandwith = 773;
+		_accel_onchip_filter_bandwith = 773;
 
 	} else {
 		return -EINVAL;
@@ -1124,6 +1125,16 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth)
 	return OK;
 }
 
+int
+LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+	_accel_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+	_accel_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+
+	return OK;
+}
+
 int
 LSM303D::accel_set_samplerate(unsigned frequency)
 {
@@ -1582,15 +1593,16 @@ reset()
 		err(1, "driver reset failed");
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-		err(1, "accel driver poll rate reset failed");
+		err(1, "accel pollrate reset failed");
 
-	int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
+	fd = open(MAG_DEVICE_PATH, O_RDONLY);
 
-	if (fd_mag < 0) {
-		warnx("could not open to mag " MAG_DEVICE_PATH);
+	if (fd < 0) {
+		warnx("mag could not be opened, external mag might be used");
 	} else {
+		/* no need to reset the mag as well, the reset() is the same */
 		if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
-			err(1, "mag driver poll rate reset failed");
+			err(1, "mag pollrate reset failed");
 	}
 
 	exit(0);
-- 
cgit v1.2.3


From fab110d21f147e5064ff140aadac649017fa466e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 21 Aug 2013 18:13:01 +0200
Subject: Moved math library to library dir, improved sensor-level HIL, cleaned
 up geo / conversion libs

---
 ROMFS/px4fmu_common/init.d/rc.hil                  |   12 +-
 makefiles/setup.mk                                 |    4 +-
 src/drivers/hott/messages.cpp                      |    2 +-
 src/drivers/lsm303d/lsm303d.cpp                    |   17 +-
 src/drivers/mpu6000/mpu6000.cpp                    |   10 +-
 src/examples/fixedwing_control/main.c              |    2 +-
 src/lib/geo/geo.c                                  |  438 ++
 src/lib/geo/geo.h                                  |  129 +
 src/lib/geo/module.mk                              |   38 +
 .../CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h       |  264 +
 .../CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h       |  265 +
 src/lib/mathlib/CMSIS/Include/arm_common_tables.h  |   93 +
 src/lib/mathlib/CMSIS/Include/arm_const_structs.h  |   85 +
 src/lib/mathlib/CMSIS/Include/arm_math.h           | 7318 ++++++++++++++++++++
 src/lib/mathlib/CMSIS/Include/core_cm3.h           | 1627 +++++
 src/lib/mathlib/CMSIS/Include/core_cm4.h           | 1772 +++++
 src/lib/mathlib/CMSIS/Include/core_cm4_simd.h      |  673 ++
 src/lib/mathlib/CMSIS/Include/core_cmFunc.h        |  636 ++
 src/lib/mathlib/CMSIS/Include/core_cmInstr.h       |  688 ++
 src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a      |  Bin 0 -> 3039508 bytes
 src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a      |  Bin 0 -> 3049684 bytes
 src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a     |  Bin 0 -> 2989192 bytes
 src/lib/mathlib/CMSIS/library.mk                   |   46 +
 src/lib/mathlib/CMSIS/license.txt                  |   27 +
 src/lib/mathlib/math/Dcm.cpp                       |  174 +
 src/lib/mathlib/math/Dcm.hpp                       |  108 +
 src/lib/mathlib/math/EulerAngles.cpp               |  126 +
 src/lib/mathlib/math/EulerAngles.hpp               |   74 +
 src/lib/mathlib/math/Limits.cpp                    |  146 +
 src/lib/mathlib/math/Limits.hpp                    |   87 +
 src/lib/mathlib/math/Matrix.cpp                    |  193 +
 src/lib/mathlib/math/Matrix.hpp                    |   61 +
 src/lib/mathlib/math/Quaternion.cpp                |  174 +
 src/lib/mathlib/math/Quaternion.hpp                |  115 +
 src/lib/mathlib/math/Vector.cpp                    |  100 +
 src/lib/mathlib/math/Vector.hpp                    |   57 +
 src/lib/mathlib/math/Vector2f.cpp                  |  103 +
 src/lib/mathlib/math/Vector2f.hpp                  |   79 +
 src/lib/mathlib/math/Vector3.cpp                   |   99 +
 src/lib/mathlib/math/Vector3.hpp                   |   76 +
 src/lib/mathlib/math/arm/Matrix.cpp                |   40 +
 src/lib/mathlib/math/arm/Matrix.hpp                |  292 +
 src/lib/mathlib/math/arm/Vector.cpp                |   40 +
 src/lib/mathlib/math/arm/Vector.hpp                |  236 +
 src/lib/mathlib/math/filter/LowPassFilter2p.cpp    |   77 +
 src/lib/mathlib/math/filter/LowPassFilter2p.hpp    |   78 +
 src/lib/mathlib/math/filter/module.mk              |   44 +
 src/lib/mathlib/math/generic/Matrix.cpp            |   40 +
 src/lib/mathlib/math/generic/Matrix.hpp            |  437 ++
 src/lib/mathlib/math/generic/Vector.cpp            |   40 +
 src/lib/mathlib/math/generic/Vector.hpp            |  245 +
 src/lib/mathlib/math/nasa_rotation_def.pdf         |  Bin 0 -> 709235 bytes
 src/lib/mathlib/math/test/test.cpp                 |   94 +
 src/lib/mathlib/math/test/test.hpp                 |   50 +
 src/lib/mathlib/math/test_math.sce                 |   63 +
 src/lib/mathlib/mathlib.h                          |   59 +
 src/lib/mathlib/module.mk                          |   62 +
 .../commander/accelerometer_calibration.cpp        |    2 +-
 src/modules/controllib/uorb/blocks.hpp             |    2 +-
 .../CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h       |  264 -
 .../CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h       |  265 -
 .../mathlib/CMSIS/Include/arm_common_tables.h      |   93 -
 .../mathlib/CMSIS/Include/arm_const_structs.h      |   85 -
 src/modules/mathlib/CMSIS/Include/arm_math.h       | 7318 --------------------
 src/modules/mathlib/CMSIS/Include/core_cm3.h       | 1627 -----
 src/modules/mathlib/CMSIS/Include/core_cm4.h       | 1772 -----
 src/modules/mathlib/CMSIS/Include/core_cm4_simd.h  |  673 --
 src/modules/mathlib/CMSIS/Include/core_cmFunc.h    |  636 --
 src/modules/mathlib/CMSIS/Include/core_cmInstr.h   |  688 --
 src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a  |  Bin 3039508 -> 0 bytes
 src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a  |  Bin 3049684 -> 0 bytes
 src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a |  Bin 2989192 -> 0 bytes
 src/modules/mathlib/CMSIS/library.mk               |   46 -
 src/modules/mathlib/CMSIS/license.txt              |   27 -
 src/modules/mathlib/math/Dcm.cpp                   |  174 -
 src/modules/mathlib/math/Dcm.hpp                   |  108 -
 src/modules/mathlib/math/EulerAngles.cpp           |  126 -
 src/modules/mathlib/math/EulerAngles.hpp           |   74 -
 src/modules/mathlib/math/Limits.cpp                |  146 -
 src/modules/mathlib/math/Limits.hpp                |   87 -
 src/modules/mathlib/math/Matrix.cpp                |  193 -
 src/modules/mathlib/math/Matrix.hpp                |   61 -
 src/modules/mathlib/math/Quaternion.cpp            |  174 -
 src/modules/mathlib/math/Quaternion.hpp            |  115 -
 src/modules/mathlib/math/Vector.cpp                |  100 -
 src/modules/mathlib/math/Vector.hpp                |   57 -
 src/modules/mathlib/math/Vector2f.cpp              |  103 -
 src/modules/mathlib/math/Vector2f.hpp              |   79 -
 src/modules/mathlib/math/Vector3.cpp               |   99 -
 src/modules/mathlib/math/Vector3.hpp               |   76 -
 src/modules/mathlib/math/arm/Matrix.cpp            |   40 -
 src/modules/mathlib/math/arm/Matrix.hpp            |  292 -
 src/modules/mathlib/math/arm/Vector.cpp            |   40 -
 src/modules/mathlib/math/arm/Vector.hpp            |  236 -
 .../mathlib/math/filter/LowPassFilter2p.cpp        |   77 -
 .../mathlib/math/filter/LowPassFilter2p.hpp        |   78 -
 src/modules/mathlib/math/filter/module.mk          |   44 -
 src/modules/mathlib/math/generic/Matrix.cpp        |   40 -
 src/modules/mathlib/math/generic/Matrix.hpp        |  437 --
 src/modules/mathlib/math/generic/Vector.cpp        |   40 -
 src/modules/mathlib/math/generic/Vector.hpp        |  245 -
 src/modules/mathlib/math/nasa_rotation_def.pdf     |  Bin 709235 -> 0 bytes
 src/modules/mathlib/math/test/test.cpp             |   94 -
 src/modules/mathlib/math/test/test.hpp             |   50 -
 src/modules/mathlib/math/test_math.sce             |   63 -
 src/modules/mathlib/mathlib.h                      |   59 -
 src/modules/mathlib/module.mk                      |   62 -
 src/modules/mavlink/mavlink.c                      |    3 +
 src/modules/mavlink/mavlink_receiver.cpp           |   88 +-
 src/modules/mavlink/orb_listener.c                 |   50 +-
 src/modules/mavlink/orb_topics.h                   |    1 +
 .../position_estimator_inav_main.c                 |    3 +-
 src/modules/systemlib/airspeed.c                   |   16 +-
 src/modules/systemlib/airspeed.h                   |    8 +
 src/modules/systemlib/conversions.c                |   97 -
 src/modules/systemlib/conversions.h                |   29 -
 src/modules/systemlib/geo/geo.c                    |  438 --
 src/modules/systemlib/geo/geo.h                    |  129 -
 src/modules/systemlib/module.mk                    |    1 -
 119 files changed, 17945 insertions(+), 17900 deletions(-)
 create mode 100644 src/lib/geo/geo.c
 create mode 100644 src/lib/geo/geo.h
 create mode 100644 src/lib/geo/module.mk
 create mode 100644 src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
 create mode 100644 src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
 create mode 100644 src/lib/mathlib/CMSIS/Include/arm_common_tables.h
 create mode 100644 src/lib/mathlib/CMSIS/Include/arm_const_structs.h
 create mode 100644 src/lib/mathlib/CMSIS/Include/arm_math.h
 create mode 100644 src/lib/mathlib/CMSIS/Include/core_cm3.h
 create mode 100644 src/lib/mathlib/CMSIS/Include/core_cm4.h
 create mode 100644 src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
 create mode 100644 src/lib/mathlib/CMSIS/Include/core_cmFunc.h
 create mode 100644 src/lib/mathlib/CMSIS/Include/core_cmInstr.h
 create mode 100644 src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
 create mode 100755 src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
 create mode 100755 src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
 create mode 100644 src/lib/mathlib/CMSIS/library.mk
 create mode 100644 src/lib/mathlib/CMSIS/license.txt
 create mode 100644 src/lib/mathlib/math/Dcm.cpp
 create mode 100644 src/lib/mathlib/math/Dcm.hpp
 create mode 100644 src/lib/mathlib/math/EulerAngles.cpp
 create mode 100644 src/lib/mathlib/math/EulerAngles.hpp
 create mode 100644 src/lib/mathlib/math/Limits.cpp
 create mode 100644 src/lib/mathlib/math/Limits.hpp
 create mode 100644 src/lib/mathlib/math/Matrix.cpp
 create mode 100644 src/lib/mathlib/math/Matrix.hpp
 create mode 100644 src/lib/mathlib/math/Quaternion.cpp
 create mode 100644 src/lib/mathlib/math/Quaternion.hpp
 create mode 100644 src/lib/mathlib/math/Vector.cpp
 create mode 100644 src/lib/mathlib/math/Vector.hpp
 create mode 100644 src/lib/mathlib/math/Vector2f.cpp
 create mode 100644 src/lib/mathlib/math/Vector2f.hpp
 create mode 100644 src/lib/mathlib/math/Vector3.cpp
 create mode 100644 src/lib/mathlib/math/Vector3.hpp
 create mode 100644 src/lib/mathlib/math/arm/Matrix.cpp
 create mode 100644 src/lib/mathlib/math/arm/Matrix.hpp
 create mode 100644 src/lib/mathlib/math/arm/Vector.cpp
 create mode 100644 src/lib/mathlib/math/arm/Vector.hpp
 create mode 100644 src/lib/mathlib/math/filter/LowPassFilter2p.cpp
 create mode 100644 src/lib/mathlib/math/filter/LowPassFilter2p.hpp
 create mode 100644 src/lib/mathlib/math/filter/module.mk
 create mode 100644 src/lib/mathlib/math/generic/Matrix.cpp
 create mode 100644 src/lib/mathlib/math/generic/Matrix.hpp
 create mode 100644 src/lib/mathlib/math/generic/Vector.cpp
 create mode 100644 src/lib/mathlib/math/generic/Vector.hpp
 create mode 100644 src/lib/mathlib/math/nasa_rotation_def.pdf
 create mode 100644 src/lib/mathlib/math/test/test.cpp
 create mode 100644 src/lib/mathlib/math/test/test.hpp
 create mode 100644 src/lib/mathlib/math/test_math.sce
 create mode 100644 src/lib/mathlib/mathlib.h
 create mode 100644 src/lib/mathlib/module.mk
 delete mode 100644 src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
 delete mode 100644 src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
 delete mode 100644 src/modules/mathlib/CMSIS/Include/arm_common_tables.h
 delete mode 100644 src/modules/mathlib/CMSIS/Include/arm_const_structs.h
 delete mode 100644 src/modules/mathlib/CMSIS/Include/arm_math.h
 delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cm3.h
 delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cm4.h
 delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
 delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cmFunc.h
 delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cmInstr.h
 delete mode 100644 src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a
 delete mode 100755 src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a
 delete mode 100755 src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a
 delete mode 100644 src/modules/mathlib/CMSIS/library.mk
 delete mode 100644 src/modules/mathlib/CMSIS/license.txt
 delete mode 100644 src/modules/mathlib/math/Dcm.cpp
 delete mode 100644 src/modules/mathlib/math/Dcm.hpp
 delete mode 100644 src/modules/mathlib/math/EulerAngles.cpp
 delete mode 100644 src/modules/mathlib/math/EulerAngles.hpp
 delete mode 100644 src/modules/mathlib/math/Limits.cpp
 delete mode 100644 src/modules/mathlib/math/Limits.hpp
 delete mode 100644 src/modules/mathlib/math/Matrix.cpp
 delete mode 100644 src/modules/mathlib/math/Matrix.hpp
 delete mode 100644 src/modules/mathlib/math/Quaternion.cpp
 delete mode 100644 src/modules/mathlib/math/Quaternion.hpp
 delete mode 100644 src/modules/mathlib/math/Vector.cpp
 delete mode 100644 src/modules/mathlib/math/Vector.hpp
 delete mode 100644 src/modules/mathlib/math/Vector2f.cpp
 delete mode 100644 src/modules/mathlib/math/Vector2f.hpp
 delete mode 100644 src/modules/mathlib/math/Vector3.cpp
 delete mode 100644 src/modules/mathlib/math/Vector3.hpp
 delete mode 100644 src/modules/mathlib/math/arm/Matrix.cpp
 delete mode 100644 src/modules/mathlib/math/arm/Matrix.hpp
 delete mode 100644 src/modules/mathlib/math/arm/Vector.cpp
 delete mode 100644 src/modules/mathlib/math/arm/Vector.hpp
 delete mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.cpp
 delete mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.hpp
 delete mode 100644 src/modules/mathlib/math/filter/module.mk
 delete mode 100644 src/modules/mathlib/math/generic/Matrix.cpp
 delete mode 100644 src/modules/mathlib/math/generic/Matrix.hpp
 delete mode 100644 src/modules/mathlib/math/generic/Vector.cpp
 delete mode 100644 src/modules/mathlib/math/generic/Vector.hpp
 delete mode 100644 src/modules/mathlib/math/nasa_rotation_def.pdf
 delete mode 100644 src/modules/mathlib/math/test/test.cpp
 delete mode 100644 src/modules/mathlib/math/test/test.hpp
 delete mode 100644 src/modules/mathlib/math/test_math.sce
 delete mode 100644 src/modules/mathlib/mathlib.h
 delete mode 100644 src/modules/mathlib/module.mk
 delete mode 100644 src/modules/systemlib/geo/geo.c
 delete mode 100644 src/modules/systemlib/geo/geo.h

(limited to 'src/drivers/lsm303d/lsm303d.cpp')

diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil
index 3517a5bd8..a843b7ffb 100644
--- a/ROMFS/px4fmu_common/init.d/rc.hil
+++ b/ROMFS/px4fmu_common/init.d/rc.hil
@@ -8,7 +8,7 @@ echo "[HIL] starting.."
 uorb start
 
 # Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/console
+mavlink start -b 230400 -d /dev/ttyS1
 
 # Create a fake HIL /dev/pwm_output interface
 hil mode_pwm
@@ -38,13 +38,13 @@ commander start
 #
 # Check if we got an IO
 #
-if [ px4io start ]
+if px4io start
 then
 	echo "IO started"
 else
 	fmu mode_serial
 	echo "FMU started"
-end
+fi
 
 #
 # Start the sensors (depends on orb, px4io)
@@ -60,9 +60,7 @@ att_pos_estimator_ekf start
 # Load mixer and start controllers (depends on px4io)
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
-fixedwing_backside start
+fw_pos_control_l1 start
+fw_att_control start
 
 echo "[HIL] setup done, running"
-
-# Exit shell to make it available to MAVLink
-exit 
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 168e41a5c..42f9a8a7f 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -43,6 +43,7 @@
 #
 export PX4_INCLUDE_DIR	 = $(abspath $(PX4_BASE)/src/include)/
 export PX4_MODULE_SRC	 = $(abspath $(PX4_BASE)/src)/
+export PX4_LIB_DIR	 = $(abspath $(PX4_BASE)/src/lib)/
 export PX4_MK_DIR	 = $(abspath $(PX4_BASE)/makefiles)/
 export NUTTX_SRC	 = $(abspath $(PX4_BASE)/NuttX/nuttx)/
 export NUTTX_APP_SRC	 = $(abspath $(PX4_BASE)/NuttX/apps)/
@@ -57,7 +58,8 @@ export ARCHIVE_DIR	 = $(abspath $(PX4_BASE)/Archives)/
 #
 export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 			   $(PX4_MODULE_SRC)/modules/ \
-			   $(PX4_INCLUDE_DIR)
+			   $(PX4_INCLUDE_DIR) \
+			   $(PX4_LIB_DIR)
 
 #
 # Tools
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 57c256339..bb8d45bea 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -42,7 +42,7 @@
 #include <math.h>
 #include <stdio.h>
 #include <string.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <unistd.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index efe7cf8eb..cf5f8d94c 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -54,7 +54,6 @@
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
-#include <systemlib/geo/geo.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
@@ -178,6 +177,8 @@ static const int ERROR = -1;
 #define LSM303D_MAG_DEFAULT_RANGE_GA			2
 #define LSM303D_MAG_DEFAULT_RATE			100
 
+#define LSM303D_ONE_G					9.80665f
+
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
 
@@ -778,7 +779,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCGRANGE:
 		/* convert to m/s^2 and return rounded in G */
-		return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
+		return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f);
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
@@ -1015,27 +1016,27 @@ LSM303D::accel_set_range(unsigned max_g)
 		max_g = 16;
 
 	if (max_g <= 2) {
-		_accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 2.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_2G_A;
 		new_scale_g_digit = 0.061e-3f;
 
 	} else if (max_g <= 4) {
-		_accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 4.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_4G_A;
 		new_scale_g_digit = 0.122e-3f;
 
 	} else if (max_g <= 6) {
-		_accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 6.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_6G_A;
 		new_scale_g_digit = 0.183e-3f;
 
 	} else if (max_g <= 8) {
-		_accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 8.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_8G_A;
 		new_scale_g_digit = 0.244e-3f;
 
 	} else if (max_g <= 16) {
-		_accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 16.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_16G_A;
 		new_scale_g_digit = 0.732e-3f;
 
@@ -1043,7 +1044,7 @@ LSM303D::accel_set_range(unsigned max_g)
 		return -EINVAL;
 	}
 
-	_accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G;
+	_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
 
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 0e65923db..14f8f44b8 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -159,6 +159,8 @@
 
 #define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ		42
 
+#define MPU6000_ONE_G					9.80665f
+
 class MPU6000_gyro;
 
 class MPU6000 : public device::SPI
@@ -543,8 +545,8 @@ void MPU6000::reset()
 
 	// Correct accel scale factors of 4096 LSB/g
 	// scale to m/s^2 ( 1g = 9.81 m/s^2)
-	_accel_range_scale = (CONSTANTS_ONE_G / 4096.0f);
-	_accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G;
+	_accel_range_scale = (MPU6000_ONE_G / 4096.0f);
+	_accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
 
 	usleep(1000);
 
@@ -906,7 +908,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _accel_range_m_s2 = 8.0f * 9.81f;
 		return -EINVAL;
 	case ACCELIOCGRANGE:
-		return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
+		return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f);
 
 	case ACCELIOCSELFTEST:
 		return accel_self_test();
@@ -1409,7 +1411,7 @@ test()
 	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
 	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
 	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
-	      (double)(a_report.range_m_s2 / CONSTANTS_ONE_G));
+	      (double)(a_report.range_m_s2 / MPU6000_ONE_G));
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index e29f76877..b286e0007 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -66,7 +66,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
new file mode 100644
index 000000000..63792dda5
--- /dev/null
+++ b/src/lib/geo/geo.c
@@ -0,0 +1,438 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 geo.c
+ *
+ * Geo / math functions to perform geodesic calculations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <geo/geo.h>
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <math.h>
+#include <stdbool.h>
+
+
+/* values for map projection */
+static double phi_1;
+static double sin_phi_1;
+static double cos_phi_1;
+static double lambda_0;
+static double scale;
+
+__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	phi_1 = lat_0 / 180.0 * M_PI;
+	lambda_0 = lon_0 / 180.0 * M_PI;
+
+	sin_phi_1 = sin(phi_1);
+	cos_phi_1 = cos(phi_1);
+
+	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
+
+	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
+	const double r_earth = 6371000;
+
+	double lat1 = phi_1;
+	double lon1 = lambda_0;
+
+	double lat2 = phi_1 + 0.5 / 180 * M_PI;
+	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
+	double sin_lat_2 = sin(lat2);
+	double cos_lat_2 = cos(lat2);
+	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
+
+	/* 2) calculate distance rho on plane */
+	double k_bar = 0;
+	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
+
+	if (0 != c)
+		k_bar = c / sin(c);
+
+	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
+	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
+	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
+
+	scale = d / rho;
+
+}
+
+__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
+{
+	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+	double phi = lat / 180.0 * M_PI;
+	double lambda = lon / 180.0 * M_PI;
+
+	double sin_phi = sin(phi);
+	double cos_phi = cos(phi);
+
+	double k_bar = 0;
+	/* using small angle approximation (formula in comment is without aproximation) */
+	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
+
+	if (0 != c)
+		k_bar = c / sin(c);
+
+	/* using small angle approximation (formula in comment is without aproximation) */
+	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
+	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
+
+//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
+}
+
+__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
+{
+	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
+
+	double x_descaled = x / scale;
+	double y_descaled = y / scale;
+
+	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
+	double sin_c = sin(c);
+	double cos_c = cos(c);
+
+	double lat_sphere = 0;
+
+	if (c != 0)
+		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
+	else
+		lat_sphere = asin(cos_c * sin_phi_1);
+
+//	printf("lat_sphere = %.10f\n",lat_sphere);
+
+	double lon_sphere = 0;
+
+	if (phi_1  == M_PI / 2) {
+		//using small angle approximation (formula in comment is without aproximation)
+		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
+
+	} else if (phi_1 == -M_PI / 2) {
+		//using small angle approximation (formula in comment is without aproximation)
+		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
+
+	} else {
+
+		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
+		//using small angle approximation
+//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
+//    	if(denominator != 0)
+//    	{
+//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
+//    	}
+//    	else
+//    	{
+//    	...
+//    	}
+	}
+
+//	printf("lon_sphere = %.10f\n",lon_sphere);
+
+	*lat = lat_sphere * 180.0 / M_PI;
+	*lon = lon_sphere * 180.0 / M_PI;
+
+}
+
+
+__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
+{
+	double lat_now_rad = lat_now / 180.0d * M_PI;
+	double lon_now_rad = lon_now / 180.0d * M_PI;
+	double lat_next_rad = lat_next / 180.0d * M_PI;
+	double lon_next_rad = lon_next / 180.0d * M_PI;
+
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
+	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
+
+	const double radius_earth = 6371000.0d;
+	return radius_earth * c; 
+}
+
+__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+	double lat_next_rad = lat_next * M_DEG_TO_RAD;
+	double lon_next_rad = lon_next * M_DEG_TO_RAD;
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	/* conscious mix of double and float trig function to maximize speed and efficiency */
+	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
+
+	theta = _wrap_pi(theta);
+
+	return theta;
+}
+
+// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
+
+__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
+{
+// This function returns the distance to the nearest point on the track line.  Distance is positive if current
+// position is right of the track and negative if left of the track as seen from a point on the track line
+// headed towards the end point.
+
+	float dist_to_end;
+	float bearing_end;
+	float bearing_track;
+	float bearing_diff;
+
+	int return_value = ERROR;	// Set error flag, cleared when valid result calculated.
+	crosstrack_error->past_end = false;
+	crosstrack_error->distance = 0.0f;
+	crosstrack_error->bearing = 0.0f;
+
+	// Return error if arguments are bad
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value;
+
+	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
+	bearing_diff = bearing_track - bearing_end;
+	bearing_diff = _wrap_pi(bearing_diff);
+
+	// Return past_end = true if past end point of line
+	if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
+		crosstrack_error->past_end = true;
+		return_value = OK;
+		return return_value;
+	}
+
+	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+	crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
+
+	if (sin(bearing_diff) >= 0) {
+		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
+
+	} else {
+		crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
+	}
+
+	return_value = OK;
+
+	return return_value;
+
+}
+
+
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+		float radius, float arc_start_bearing, float arc_sweep)
+{
+	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
+	// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
+	// headed towards the end point.
+
+	// Determine if the current position is inside or outside the sector between the line from the center
+	// to the arc start and the line from the center to the arc end
+	float	bearing_sector_start;
+	float	bearing_sector_end;
+	float	bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
+	bool	in_sector;
+
+	int return_value = ERROR;		// Set error flag, cleared when valid result calculated.
+	crosstrack_error->past_end = false;
+	crosstrack_error->distance = 0.0f;
+	crosstrack_error->bearing = 0.0f;
+
+	// Return error if arguments are bad
+	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value;
+
+
+	if (arc_sweep >= 0) {
+		bearing_sector_start = arc_start_bearing;
+		bearing_sector_end = arc_start_bearing + arc_sweep;
+
+		if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
+
+	} else {
+		bearing_sector_end = arc_start_bearing;
+		bearing_sector_start = arc_start_bearing - arc_sweep;
+
+		if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F;
+	}
+
+	in_sector = false;
+
+	// Case where sector does not span zero
+	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
+
+	// Case where sector does span zero
+	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
+
+	// If in the sector then calculate distance and bearing to closest point
+	if (in_sector) {
+		crosstrack_error->past_end = false;
+		float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
+
+		if (dist_to_center <= radius) {
+			crosstrack_error->distance = radius - dist_to_center;
+			crosstrack_error->bearing = bearing_now + M_PI_F;
+
+		} else {
+			crosstrack_error->distance = dist_to_center - radius;
+			crosstrack_error->bearing = bearing_now;
+		}
+
+		// If out of the sector then calculate dist and bearing to start or end point
+
+	} else {
+
+		// Use the approximation  that 111,111 meters in the y direction is 1 degree (of latitude)
+		// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
+		// calculate the position of the start and end points.  We should not be doing this often
+		// as this function generally will not be called repeatedly when we are out of the sector.
+
+		// TO DO - this is messed up and won't compile
+		float start_disp_x = radius * sin(arc_start_bearing);
+		float start_disp_y = radius * cos(arc_start_bearing);
+		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
+		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
+		float lon_start = lon_now + start_disp_x / 111111.0d;
+		float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
+		float lon_end = lon_now + end_disp_x / 111111.0d;
+		float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
+		float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+		float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+
+
+		if (dist_to_start < dist_to_end) {
+			crosstrack_error->distance = dist_to_start;
+			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+
+		} else {
+			crosstrack_error->past_end = true;
+			crosstrack_error->distance = dist_to_end;
+			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+		}
+
+	}
+
+	crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
+	return_value = OK;
+	return return_value;
+}
+
+__EXPORT float _wrap_pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing) || bearing == 0) {
+		return bearing;
+	}
+
+	int c = 0;
+
+	while (bearing > M_PI_F && c < 30) {
+		bearing -= M_TWOPI_F;
+		c++;
+	}
+
+	c = 0;
+
+	while (bearing <=  -M_PI_F && c < 30) {
+		bearing += M_TWOPI_F;
+		c++;
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_2pi(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	while (bearing >= M_TWOPI_F) {
+		bearing = bearing - M_TWOPI_F;
+	}
+
+	while (bearing <  0.0f) {
+		bearing = bearing + M_TWOPI_F;
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_180(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	while (bearing > 180.0f) {
+		bearing = bearing - 360.0f;
+	}
+
+	while (bearing <=  -180.0f) {
+		bearing = bearing + 360.0f;
+	}
+
+	return bearing;
+}
+
+__EXPORT float _wrap_360(float bearing)
+{
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
+
+	while (bearing >= 360.0f) {
+		bearing = bearing - 360.0f;
+	}
+
+	while (bearing <  0.0f) {
+		bearing = bearing + 360.0f;
+	}
+
+	return bearing;
+}
+
+
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
new file mode 100644
index 000000000..dadec51ec
--- /dev/null
+++ b/src/lib/geo/geo.h
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 geo.h
+ *
+ * Definition of geo / math functions to perform geodesic calculations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
+ */
+
+#pragma once
+
+__BEGIN_DECLS
+
+#include <stdbool.h>
+
+#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
+#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
+#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
+#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
+#define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
+
+/* compatibility aliases */
+#define RADIUS_OF_EARTH		CONSTANTS_RADIUS_OF_EARTH
+#define GRAVITY_MSS		CONSTANTS_ONE_G
+
+// XXX remove
+struct crosstrack_error_s {
+	bool past_end;		// Flag indicating we are past the end of the line/arc segment
+	float distance;		// Distance in meters to closest point on line/arc
+	float bearing;		// Bearing in radians to closest point on line/arc
+} ;
+
+/**
+ * Initializes the map transformation.
+ *
+ * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_init(double lat_0, double lon_0);
+
+/**
+ * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
+
+/**
+ * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
+ *
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
+
+/**
+ * Returns the distance to the next waypoint in meters.
+ *
+ * @param lat_now current position in degrees (47.1234567°, not 471234567°)
+ * @param lon_now current position in degrees (8.1234567°, not 81234567°)
+ * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
+ * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
+
+/**
+ * Returns the bearing to the next waypoint in radians.
+ *
+ * @param lat_now current position in degrees (47.1234567°, not 471234567°)
+ * @param lon_now current position in degrees (8.1234567°, not 81234567°)
+ * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
+ * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
+
+__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
+
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+		float radius, float arc_start_bearing, float arc_sweep);
+
+__EXPORT float _wrap_180(float bearing);
+__EXPORT float _wrap_360(float bearing);
+__EXPORT float _wrap_pi(float bearing);
+__EXPORT float _wrap_2pi(float bearing);
+
+__END_DECLS
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
new file mode 100644
index 000000000..30a2dc99f
--- /dev/null
+++ b/src/lib/geo/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Geo library
+#
+
+SRCS		 = geo.c
diff --git a/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
new file mode 100644
index 000000000..8f39acd9d
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
@@ -0,0 +1,264 @@
+/**************************************************************************//**
+ * @file     ARMCM3.h
+ * @brief    CMSIS Core Peripheral Access Layer Header File for
+ *           ARMCM3 Device Series
+ * @version  V1.07
+ * @date     30. January 2012
+ *
+ * @note
+ * Copyright (C) 2012 ARM Limited. All rights reserved.
+ *
+ * @par
+ * ARM Limited (ARM) is supplying this software for use with Cortex-M 
+ * processor based microcontrollers.  This file can be freely distributed 
+ * within development tools that are supporting such ARM based processors. 
+ *
+ * @par
+ * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
+ * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
+ * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
+ * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
+ *
+ ******************************************************************************/
+
+#ifndef ARMCM3_H
+#define ARMCM3_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* -------------------------  Interrupt Number Definition  ------------------------ */
+
+typedef enum IRQn
+{
+/* -------------------  Cortex-M3 Processor Exceptions Numbers  ------------------- */
+  NonMaskableInt_IRQn           = -14,      /*!<  2 Non Maskable Interrupt          */
+  HardFault_IRQn                = -13,      /*!<  3 HardFault Interrupt             */
+  MemoryManagement_IRQn         = -12,      /*!<  4 Memory Management Interrupt     */
+  BusFault_IRQn                 = -11,      /*!<  5 Bus Fault Interrupt             */
+  UsageFault_IRQn               = -10,      /*!<  6 Usage Fault Interrupt           */
+  SVCall_IRQn                   =  -5,      /*!< 11 SV Call Interrupt               */
+  DebugMonitor_IRQn             =  -4,      /*!< 12 Debug Monitor Interrupt         */
+  PendSV_IRQn                   =  -2,      /*!< 14 Pend SV Interrupt               */
+  SysTick_IRQn                  =  -1,      /*!< 15 System Tick Interrupt           */
+
+/* ----------------------  ARMCM3 Specific Interrupt Numbers  --------------------- */
+  WDT_IRQn                      =   0,      /*!< Watchdog Timer Interrupt           */
+  RTC_IRQn                      =   1,      /*!< Real Time Clock Interrupt          */
+  TIM0_IRQn                     =   2,      /*!< Timer0 / Timer1 Interrupt          */
+  TIM2_IRQn                     =   3,      /*!< Timer2 / Timer3 Interrupt          */
+  MCIA_IRQn                     =   4,      /*!< MCIa Interrupt                     */
+  MCIB_IRQn                     =   5,      /*!< MCIb Interrupt                     */
+  UART0_IRQn                    =   6,      /*!< UART0 Interrupt                    */
+  UART1_IRQn                    =   7,      /*!< UART1 Interrupt                    */
+  UART2_IRQn                    =   8,      /*!< UART2 Interrupt                    */
+  UART4_IRQn                    =   9,      /*!< UART4 Interrupt                    */
+  AACI_IRQn                     =  10,      /*!< AACI / AC97 Interrupt              */
+  CLCD_IRQn                     =  11,      /*!< CLCD Combined Interrupt            */
+  ENET_IRQn                     =  12,      /*!< Ethernet Interrupt                 */
+  USBDC_IRQn                    =  13,      /*!< USB Device Interrupt               */
+  USBHC_IRQn                    =  14,      /*!< USB Host Controller Interrupt      */
+  CHLCD_IRQn                    =  15,      /*!< Character LCD Interrupt            */
+  FLEXRAY_IRQn                  =  16,      /*!< Flexray Interrupt                  */
+  CAN_IRQn                      =  17,      /*!< CAN Interrupt                      */
+  LIN_IRQn                      =  18,      /*!< LIN Interrupt                      */
+  I2C_IRQn                      =  19,      /*!< I2C ADC/DAC Interrupt              */
+  CPU_CLCD_IRQn                 =  28,      /*!< CPU CLCD Combined Interrupt        */
+  UART3_IRQn                    =  30,      /*!< UART3 Interrupt                    */
+  SPI_IRQn                      =  31,      /*!< SPI Touchscreen Interrupt          */
+} IRQn_Type;
+
+
+/* ================================================================================ */
+/* ================      Processor and Core Peripheral Section     ================ */
+/* ================================================================================ */
+
+/* --------  Configuration of the Cortex-M4 Processor and Core Peripherals  ------- */
+#define __CM3_REV                 0x0201    /*!< Core revision r2p1                              */
+#define __MPU_PRESENT             1         /*!< MPU present or not                              */
+#define __NVIC_PRIO_BITS          3         /*!< Number of Bits used for Priority Levels         */
+#define __Vendor_SysTickConfig    0         /*!< Set to 1 if different SysTick Config is used    */
+
+#include <core_cm3.h>                       /* Processor and core peripherals                    */
+/* NuttX */
+//#include "system_ARMCM3.h"                  /* System Header                                     */
+
+
+/* ================================================================================ */
+/* ================       Device Specific Peripheral Section       ================ */
+/* ================================================================================ */
+
+/* -------------------  Start of section using anonymous unions  ------------------ */
+#if defined(__CC_ARM)
+  #pragma push
+  #pragma anon_unions
+#elif defined(__ICCARM__)
+  #pragma language=extended
+#elif defined(__GNUC__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+/* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+  #pragma warning 586
+#else
+  #warning Not supported compiler type
+#endif
+
+
+
+/* ================================================================================ */
+/* ================            CPU FPGA System (CPU_SYS)           ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
+  __IO uint32_t MEMCFG;           /* Offset: 0x004 (R/W)  Remap and Alias Memory Control */
+  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
+  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
+  __I  uint32_t TS;               /* Offset: 0x010 (R/ )  Touchscreen Register */
+  __IO uint32_t CTRL1;            /* Offset: 0x014 (R/W)  Misc Control Functions */
+       uint32_t RESERVED0[2];
+  __IO uint32_t CLKCFG;           /* Offset: 0x020 (R/W)  System Clock Configuration */
+  __IO uint32_t WSCFG;            /* Offset: 0x024 (R/W)  Flash Waitstate Configuration */
+  __IO uint32_t CPUCFG;           /* Offset: 0x028 (R/W)  Processor Configuration */
+       uint32_t RESERVED1[3];
+  __IO uint32_t BASE;             /* Offset: 0x038 (R/W)  ROM Table base Address */
+  __IO uint32_t ID2;              /* Offset: 0x03C (R/W)  Secondary Identification Register */
+} ARM_CPU_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================            DUT FPGA System (DUT_SYS)           ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
+  __IO uint32_t PERCFG;           /* Offset: 0x004 (R/W)  Peripheral Control Signals */
+  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
+  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
+  __IO uint32_t SEG7;             /* Offset: 0x010 (R/W)  7-segment LED Output States */
+  __I  uint32_t CNT25MHz;         /* Offset: 0x014 (R/ )  Freerunning counter incrementing at 25MHz */
+  __I  uint32_t CNT100Hz;         /* Offset: 0x018 (R/ )  Freerunning counter incrementing at 100Hz */
+} ARM_DUT_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================                   Timer (TIM)                  ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __IO uint32_t Timer1Load;       /* Offset: 0x000 (R/W)  Timer 1 Load */
+  __I  uint32_t Timer1Value;      /* Offset: 0x004 (R/ )  Timer 1 Counter Current Value */
+  __IO uint32_t Timer1Control;    /* Offset: 0x008 (R/W)  Timer 1 Control */
+  __O  uint32_t Timer1IntClr;     /* Offset: 0x00C ( /W)  Timer 1 Interrupt Clear */
+  __I  uint32_t Timer1RIS;        /* Offset: 0x010 (R/ )  Timer 1 Raw Interrupt Status */
+  __I  uint32_t Timer1MIS;        /* Offset: 0x014 (R/ )  Timer 1 Masked Interrupt Status */
+  __IO uint32_t Timer1BGLoad;     /* Offset: 0x018 (R/W)  Background Load Register */
+       uint32_t RESERVED0[1];
+  __IO uint32_t Timer2Load;       /* Offset: 0x020 (R/W)  Timer 2 Load */
+  __I  uint32_t Timer2Value;      /* Offset: 0x024 (R/ )  Timer 2 Counter Current Value */
+  __IO uint32_t Timer2Control;    /* Offset: 0x028 (R/W)  Timer 2 Control */
+  __O  uint32_t Timer2IntClr;     /* Offset: 0x02C ( /W)  Timer 2 Interrupt Clear */
+  __I  uint32_t Timer2RIS;        /* Offset: 0x030 (R/ )  Timer 2 Raw Interrupt Status */
+  __I  uint32_t Timer2MIS;        /* Offset: 0x034 (R/ )  Timer 2 Masked Interrupt Status */
+  __IO uint32_t Timer2BGLoad;     /* Offset: 0x038 (R/W)  Background Load Register */
+} ARM_TIM_TypeDef;
+
+
+/* ================================================================================ */
+/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
+/* ================================================================================ */
+typedef struct
+{
+  __IO uint32_t DR;               /* Offset: 0x000 (R/W)  Data */ 
+  union {
+  __I  uint32_t RSR;              /* Offset: 0x000 (R/ )  Receive Status */
+  __O  uint32_t ECR;              /* Offset: 0x000 ( /W)  Error Clear */
+  };
+       uint32_t RESERVED0[4];
+  __IO uint32_t FR;               /* Offset: 0x018 (R/W)  Flags */
+       uint32_t RESERVED1[1];
+  __IO uint32_t ILPR;             /* Offset: 0x020 (R/W)  IrDA Low-power Counter */
+  __IO uint32_t IBRD;             /* Offset: 0x024 (R/W)  Interger Baud Rate */
+  __IO uint32_t FBRD;             /* Offset: 0x028 (R/W)  Fractional Baud Rate */
+  __IO uint32_t LCR_H;            /* Offset: 0x02C (R/W)  Line Control */
+  __IO uint32_t CR;               /* Offset: 0x030 (R/W)  Control */
+  __IO uint32_t IFLS;             /* Offset: 0x034 (R/W)  Interrupt FIFO Level Select */
+  __IO uint32_t IMSC;             /* Offset: 0x038 (R/W)  Interrupt Mask Set / Clear */
+  __IO uint32_t RIS;              /* Offset: 0x03C (R/W)  Raw Interrupt Status */
+  __IO uint32_t MIS;              /* Offset: 0x040 (R/W)  Masked Interrupt Status */
+  __O  uint32_t ICR;              /* Offset: 0x044 ( /W)  Interrupt Clear */
+  __IO uint32_t DMACR;            /* Offset: 0x048 (R/W)  DMA Control */
+} ARM_UART_TypeDef;
+
+
+/* --------------------  End of section using anonymous unions  ------------------- */
+#if defined(__CC_ARM)
+  #pragma pop
+#elif defined(__ICCARM__)
+  /* leave anonymous unions enabled */
+#elif defined(__GNUC__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+  #pragma warning restore
+#else
+  #warning Not supported compiler type
+#endif
+
+
+
+
+/* ================================================================================ */
+/* ================              Peripheral memory map             ================ */
+/* ================================================================================ */
+/* --------------------------  CPU FPGA memory map  ------------------------------- */
+#define ARM_FLASH_BASE            (0x00000000UL)
+#define ARM_RAM_BASE              (0x20000000UL)
+#define ARM_RAM_FPGA_BASE         (0x1EFF0000UL)
+#define ARM_CPU_CFG_BASE          (0xDFFF0000UL)
+
+#define ARM_CPU_SYS_BASE          (ARM_CPU_CFG_BASE  + 0x00000)
+#define ARM_UART3_BASE            (ARM_CPU_CFG_BASE  + 0x05000)
+
+/* --------------------------  DUT FPGA memory map  ------------------------------- */
+#define ARM_APB_BASE              (0x40000000UL)
+#define ARM_AHB_BASE              (0x4FF00000UL)
+#define ARM_DMC_BASE              (0x60000000UL)
+#define ARM_SMC_BASE              (0xA0000000UL)
+
+#define ARM_TIM0_BASE             (ARM_APB_BASE      + 0x02000)
+#define ARM_TIM2_BASE             (ARM_APB_BASE      + 0x03000)
+#define ARM_DUT_SYS_BASE          (ARM_APB_BASE      + 0x04000)
+#define ARM_UART0_BASE            (ARM_APB_BASE      + 0x06000)
+#define ARM_UART1_BASE            (ARM_APB_BASE      + 0x07000)
+#define ARM_UART2_BASE            (ARM_APB_BASE      + 0x08000)
+#define ARM_UART4_BASE            (ARM_APB_BASE      + 0x09000)
+
+
+/* ================================================================================ */
+/* ================             Peripheral declaration             ================ */
+/* ================================================================================ */
+/* --------------------------  CPU FPGA Peripherals  ------------------------------ */
+#define ARM_CPU_SYS               ((ARM_CPU_SYS_TypeDef *)  ARM_CPU_SYS_BASE)
+#define ARM_UART3                 ((   ARM_UART_TypeDef *)    ARM_UART3_BASE)
+
+/* --------------------------  DUT FPGA Peripherals  ------------------------------ */
+#define ARM_DUT_SYS               ((ARM_DUT_SYS_TypeDef *)  ARM_DUT_SYS_BASE)
+#define ARM_TIM0                  ((    ARM_TIM_TypeDef *)     ARM_TIM0_BASE)
+#define ARM_TIM2                  ((    ARM_TIM_TypeDef *)     ARM_TIM2_BASE)
+#define ARM_UART0                 ((   ARM_UART_TypeDef *)    ARM_UART0_BASE)
+#define ARM_UART1                 ((   ARM_UART_TypeDef *)    ARM_UART1_BASE)
+#define ARM_UART2                 ((   ARM_UART_TypeDef *)    ARM_UART2_BASE)
+#define ARM_UART4                 ((   ARM_UART_TypeDef *)    ARM_UART4_BASE)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* ARMCM3_H */
diff --git a/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
new file mode 100644
index 000000000..181b7e433
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
@@ -0,0 +1,265 @@
+/**************************************************************************//**
+ * @file     ARMCM4.h
+ * @brief    CMSIS Core Peripheral Access Layer Header File for
+ *           ARMCM4 Device Series
+ * @version  V1.07
+ * @date     30. January 2012
+ *
+ * @note
+ * Copyright (C) 2012 ARM Limited. All rights reserved.
+ *
+ * @par
+ * ARM Limited (ARM) is supplying this software for use with Cortex-M 
+ * processor based microcontrollers.  This file can be freely distributed 
+ * within development tools that are supporting such ARM based processors. 
+ *
+ * @par
+ * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
+ * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
+ * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
+ * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
+ *
+ ******************************************************************************/
+
+#ifndef ARMCM4_H
+#define ARMCM4_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* -------------------------  Interrupt Number Definition  ------------------------ */
+
+typedef enum IRQn
+{
+/* -------------------  Cortex-M4 Processor Exceptions Numbers  ------------------- */
+  NonMaskableInt_IRQn           = -14,      /*!<  2 Non Maskable Interrupt          */
+  HardFault_IRQn                = -13,      /*!<  3 HardFault Interrupt             */
+  MemoryManagement_IRQn         = -12,      /*!<  4 Memory Management Interrupt     */
+  BusFault_IRQn                 = -11,      /*!<  5 Bus Fault Interrupt             */
+  UsageFault_IRQn               = -10,      /*!<  6 Usage Fault Interrupt           */
+  SVCall_IRQn                   =  -5,      /*!< 11 SV Call Interrupt               */
+  DebugMonitor_IRQn             =  -4,      /*!< 12 Debug Monitor Interrupt         */
+  PendSV_IRQn                   =  -2,      /*!< 14 Pend SV Interrupt               */
+  SysTick_IRQn                  =  -1,      /*!< 15 System Tick Interrupt           */
+
+/* ----------------------  ARMCM4 Specific Interrupt Numbers  --------------------- */
+  WDT_IRQn                      =   0,      /*!< Watchdog Timer Interrupt           */
+  RTC_IRQn                      =   1,      /*!< Real Time Clock Interrupt          */
+  TIM0_IRQn                     =   2,      /*!< Timer0 / Timer1 Interrupt          */
+  TIM2_IRQn                     =   3,      /*!< Timer2 / Timer3 Interrupt          */
+  MCIA_IRQn                     =   4,      /*!< MCIa Interrupt                     */
+  MCIB_IRQn                     =   5,      /*!< MCIb Interrupt                     */
+  UART0_IRQn                    =   6,      /*!< UART0 Interrupt                    */
+  UART1_IRQn                    =   7,      /*!< UART1 Interrupt                    */
+  UART2_IRQn                    =   8,      /*!< UART2 Interrupt                    */
+  UART4_IRQn                    =   9,      /*!< UART4 Interrupt                    */
+  AACI_IRQn                     =  10,      /*!< AACI / AC97 Interrupt              */
+  CLCD_IRQn                     =  11,      /*!< CLCD Combined Interrupt            */
+  ENET_IRQn                     =  12,      /*!< Ethernet Interrupt                 */
+  USBDC_IRQn                    =  13,      /*!< USB Device Interrupt               */
+  USBHC_IRQn                    =  14,      /*!< USB Host Controller Interrupt      */
+  CHLCD_IRQn                    =  15,      /*!< Character LCD Interrupt            */
+  FLEXRAY_IRQn                  =  16,      /*!< Flexray Interrupt                  */
+  CAN_IRQn                      =  17,      /*!< CAN Interrupt                      */
+  LIN_IRQn                      =  18,      /*!< LIN Interrupt                      */
+  I2C_IRQn                      =  19,      /*!< I2C ADC/DAC Interrupt              */
+  CPU_CLCD_IRQn                 =  28,      /*!< CPU CLCD Combined Interrupt        */
+  UART3_IRQn                    =  30,      /*!< UART3 Interrupt                    */
+  SPI_IRQn                      =  31,      /*!< SPI Touchscreen Interrupt          */
+} IRQn_Type;
+
+
+/* ================================================================================ */
+/* ================      Processor and Core Peripheral Section     ================ */
+/* ================================================================================ */
+
+/* --------  Configuration of the Cortex-M4 Processor and Core Peripherals  ------- */
+#define __CM4_REV                 0x0001    /*!< Core revision r0p1                              */
+#define __MPU_PRESENT             1         /*!< MPU present or not                              */
+#define __NVIC_PRIO_BITS          3         /*!< Number of Bits used for Priority Levels         */
+#define __Vendor_SysTickConfig    0         /*!< Set to 1 if different SysTick Config is used    */
+#define __FPU_PRESENT             1         /*!< FPU present or not                              */
+
+#include <core_cm4.h>                       /* Processor and core peripherals                    */
+/* NuttX */
+//#include "system_ARMCM4.h"                  /* System Header                                     */
+
+
+/* ================================================================================ */
+/* ================       Device Specific Peripheral Section       ================ */
+/* ================================================================================ */
+
+/* -------------------  Start of section using anonymous unions  ------------------ */
+#if defined(__CC_ARM)
+  #pragma push
+  #pragma anon_unions
+#elif defined(__ICCARM__)
+  #pragma language=extended
+#elif defined(__GNUC__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+/* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+  #pragma warning 586
+#else
+  #warning Not supported compiler type
+#endif
+
+
+
+/* ================================================================================ */
+/* ================            CPU FPGA System (CPU_SYS)           ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
+  __IO uint32_t MEMCFG;           /* Offset: 0x004 (R/W)  Remap and Alias Memory Control */
+  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
+  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
+  __I  uint32_t TS;               /* Offset: 0x010 (R/ )  Touchscreen Register */
+  __IO uint32_t CTRL1;            /* Offset: 0x014 (R/W)  Misc Control Functions */
+       uint32_t RESERVED0[2];
+  __IO uint32_t CLKCFG;           /* Offset: 0x020 (R/W)  System Clock Configuration */
+  __IO uint32_t WSCFG;            /* Offset: 0x024 (R/W)  Flash Waitstate Configuration */
+  __IO uint32_t CPUCFG;           /* Offset: 0x028 (R/W)  Processor Configuration */
+       uint32_t RESERVED1[3];
+  __IO uint32_t BASE;             /* Offset: 0x038 (R/W)  ROM Table base Address */
+  __IO uint32_t ID2;              /* Offset: 0x03C (R/W)  Secondary Identification Register */
+} ARM_CPU_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================            DUT FPGA System (DUT_SYS)           ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
+  __IO uint32_t PERCFG;           /* Offset: 0x004 (R/W)  Peripheral Control Signals */
+  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
+  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
+  __IO uint32_t SEG7;             /* Offset: 0x010 (R/W)  7-segment LED Output States */
+  __I  uint32_t CNT25MHz;         /* Offset: 0x014 (R/ )  Freerunning counter incrementing at 25MHz */
+  __I  uint32_t CNT100Hz;         /* Offset: 0x018 (R/ )  Freerunning counter incrementing at 100Hz */
+} ARM_DUT_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================                   Timer (TIM)                  ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __IO uint32_t Timer1Load;       /* Offset: 0x000 (R/W)  Timer 1 Load */
+  __I  uint32_t Timer1Value;      /* Offset: 0x004 (R/ )  Timer 1 Counter Current Value */
+  __IO uint32_t Timer1Control;    /* Offset: 0x008 (R/W)  Timer 1 Control */
+  __O  uint32_t Timer1IntClr;     /* Offset: 0x00C ( /W)  Timer 1 Interrupt Clear */
+  __I  uint32_t Timer1RIS;        /* Offset: 0x010 (R/ )  Timer 1 Raw Interrupt Status */
+  __I  uint32_t Timer1MIS;        /* Offset: 0x014 (R/ )  Timer 1 Masked Interrupt Status */
+  __IO uint32_t Timer1BGLoad;     /* Offset: 0x018 (R/W)  Background Load Register */
+       uint32_t RESERVED0[1];
+  __IO uint32_t Timer2Load;       /* Offset: 0x020 (R/W)  Timer 2 Load */
+  __I  uint32_t Timer2Value;      /* Offset: 0x024 (R/ )  Timer 2 Counter Current Value */
+  __IO uint32_t Timer2Control;    /* Offset: 0x028 (R/W)  Timer 2 Control */
+  __O  uint32_t Timer2IntClr;     /* Offset: 0x02C ( /W)  Timer 2 Interrupt Clear */
+  __I  uint32_t Timer2RIS;        /* Offset: 0x030 (R/ )  Timer 2 Raw Interrupt Status */
+  __I  uint32_t Timer2MIS;        /* Offset: 0x034 (R/ )  Timer 2 Masked Interrupt Status */
+  __IO uint32_t Timer2BGLoad;     /* Offset: 0x038 (R/W)  Background Load Register */
+} ARM_TIM_TypeDef;
+
+
+/* ================================================================================ */
+/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
+/* ================================================================================ */
+typedef struct
+{
+  __IO uint32_t DR;               /* Offset: 0x000 (R/W)  Data */ 
+  union {
+  __I  uint32_t RSR;              /* Offset: 0x000 (R/ )  Receive Status */
+  __O  uint32_t ECR;              /* Offset: 0x000 ( /W)  Error Clear */
+  };
+       uint32_t RESERVED0[4];
+  __IO uint32_t FR;               /* Offset: 0x018 (R/W)  Flags */
+       uint32_t RESERVED1[1];
+  __IO uint32_t ILPR;             /* Offset: 0x020 (R/W)  IrDA Low-power Counter */
+  __IO uint32_t IBRD;             /* Offset: 0x024 (R/W)  Interger Baud Rate */
+  __IO uint32_t FBRD;             /* Offset: 0x028 (R/W)  Fractional Baud Rate */
+  __IO uint32_t LCR_H;            /* Offset: 0x02C (R/W)  Line Control */
+  __IO uint32_t CR;               /* Offset: 0x030 (R/W)  Control */
+  __IO uint32_t IFLS;             /* Offset: 0x034 (R/W)  Interrupt FIFO Level Select */
+  __IO uint32_t IMSC;             /* Offset: 0x038 (R/W)  Interrupt Mask Set / Clear */
+  __IO uint32_t RIS;              /* Offset: 0x03C (R/W)  Raw Interrupt Status */
+  __IO uint32_t MIS;              /* Offset: 0x040 (R/W)  Masked Interrupt Status */
+  __O  uint32_t ICR;              /* Offset: 0x044 ( /W)  Interrupt Clear */
+  __IO uint32_t DMACR;            /* Offset: 0x048 (R/W)  DMA Control */
+} ARM_UART_TypeDef;
+
+
+/* --------------------  End of section using anonymous unions  ------------------- */
+#if defined(__CC_ARM)
+  #pragma pop
+#elif defined(__ICCARM__)
+  /* leave anonymous unions enabled */
+#elif defined(__GNUC__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+  #pragma warning restore
+#else
+  #warning Not supported compiler type
+#endif
+
+
+
+
+/* ================================================================================ */
+/* ================              Peripheral memory map             ================ */
+/* ================================================================================ */
+/* --------------------------  CPU FPGA memory map  ------------------------------- */
+#define ARM_FLASH_BASE            (0x00000000UL)
+#define ARM_RAM_BASE              (0x20000000UL)
+#define ARM_RAM_FPGA_BASE         (0x1EFF0000UL)
+#define ARM_CPU_CFG_BASE          (0xDFFF0000UL)
+
+#define ARM_CPU_SYS_BASE          (ARM_CPU_CFG_BASE  + 0x00000)
+#define ARM_UART3_BASE            (ARM_CPU_CFG_BASE  + 0x05000)
+
+/* --------------------------  DUT FPGA memory map  ------------------------------- */
+#define ARM_APB_BASE              (0x40000000UL)
+#define ARM_AHB_BASE              (0x4FF00000UL)
+#define ARM_DMC_BASE              (0x60000000UL)
+#define ARM_SMC_BASE              (0xA0000000UL)
+
+#define ARM_TIM0_BASE             (ARM_APB_BASE      + 0x02000)
+#define ARM_TIM2_BASE             (ARM_APB_BASE      + 0x03000)
+#define ARM_DUT_SYS_BASE          (ARM_APB_BASE      + 0x04000)
+#define ARM_UART0_BASE            (ARM_APB_BASE      + 0x06000)
+#define ARM_UART1_BASE            (ARM_APB_BASE      + 0x07000)
+#define ARM_UART2_BASE            (ARM_APB_BASE      + 0x08000)
+#define ARM_UART4_BASE            (ARM_APB_BASE      + 0x09000)
+
+
+/* ================================================================================ */
+/* ================             Peripheral declaration             ================ */
+/* ================================================================================ */
+/* --------------------------  CPU FPGA Peripherals  ------------------------------ */
+#define ARM_CPU_SYS               ((ARM_CPU_SYS_TypeDef *)  ARM_CPU_SYS_BASE)
+#define ARM_UART3                 ((   ARM_UART_TypeDef *)    ARM_UART3_BASE)
+
+/* --------------------------  DUT FPGA Peripherals  ------------------------------ */
+#define ARM_DUT_SYS               ((ARM_DUT_SYS_TypeDef *)  ARM_DUT_SYS_BASE)
+#define ARM_TIM0                  ((    ARM_TIM_TypeDef *)     ARM_TIM0_BASE)
+#define ARM_TIM2                  ((    ARM_TIM_TypeDef *)     ARM_TIM2_BASE)
+#define ARM_UART0                 ((   ARM_UART_TypeDef *)    ARM_UART0_BASE)
+#define ARM_UART1                 ((   ARM_UART_TypeDef *)    ARM_UART1_BASE)
+#define ARM_UART2                 ((   ARM_UART_TypeDef *)    ARM_UART2_BASE)
+#define ARM_UART4                 ((   ARM_UART_TypeDef *)    ARM_UART4_BASE)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* ARMCM4_H */
diff --git a/src/lib/mathlib/CMSIS/Include/arm_common_tables.h b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h
new file mode 100644
index 000000000..9c37ab4e5
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h
@@ -0,0 +1,93 @@
+/* ----------------------------------------------------------------------
+* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+*
+* $Date:        17. January 2013
+* $Revision:    V1.4.1
+*
+* Project:      CMSIS DSP Library
+* Title:        arm_common_tables.h
+*
+* Description:  This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions
+*
+* Target Processor: Cortex-M4/Cortex-M3
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*   - Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   - 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.
+*   - Neither the name of ARM LIMITED nor the names of its contributors
+*     may be used to endorse or promote products derived from this
+*     software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+* -------------------------------------------------------------------- */
+
+#ifndef _ARM_COMMON_TABLES_H
+#define _ARM_COMMON_TABLES_H
+
+#include "arm_math.h"
+
+extern const uint16_t armBitRevTable[1024];
+extern const q15_t armRecipTableQ15[64];
+extern const q31_t armRecipTableQ31[64];
+extern const q31_t realCoefAQ31[1024];
+extern const q31_t realCoefBQ31[1024];
+extern const float32_t twiddleCoef_16[32];
+extern const float32_t twiddleCoef_32[64];
+extern const float32_t twiddleCoef_64[128];
+extern const float32_t twiddleCoef_128[256];
+extern const float32_t twiddleCoef_256[512];
+extern const float32_t twiddleCoef_512[1024];
+extern const float32_t twiddleCoef_1024[2048];
+extern const float32_t twiddleCoef_2048[4096];
+extern const float32_t twiddleCoef_4096[8192];
+#define twiddleCoef twiddleCoef_4096
+extern const q31_t twiddleCoefQ31[6144];
+extern const q15_t twiddleCoefQ15[6144];
+extern const float32_t twiddleCoef_rfft_32[32];
+extern const float32_t twiddleCoef_rfft_64[64];
+extern const float32_t twiddleCoef_rfft_128[128];
+extern const float32_t twiddleCoef_rfft_256[256];
+extern const float32_t twiddleCoef_rfft_512[512];
+extern const float32_t twiddleCoef_rfft_1024[1024];
+extern const float32_t twiddleCoef_rfft_2048[2048];
+extern const float32_t twiddleCoef_rfft_4096[4096];
+
+
+#define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20  )
+#define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48  )
+#define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56  )
+#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 )
+#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 )
+#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 )
+#define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800)
+#define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808)
+#define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032)
+
+extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH];
+
+#endif /*  ARM_COMMON_TABLES_H */
diff --git a/src/lib/mathlib/CMSIS/Include/arm_const_structs.h b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h
new file mode 100644
index 000000000..406f737dc
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h
@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------
+* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+*
+* $Date:        17. January 2013
+* $Revision:    V1.4.1
+*
+* Project:      CMSIS DSP Library
+* Title:        arm_const_structs.h
+*
+* Description:  This file has constant structs that are initialized for
+*               user convenience.  For example, some can be given as
+*               arguments to the arm_cfft_f32() function.
+*
+* Target Processor: Cortex-M4/Cortex-M3
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*   - Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   - 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.
+*   - Neither the name of ARM LIMITED nor the names of its contributors
+*     may be used to endorse or promote products derived from this
+*     software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+* -------------------------------------------------------------------- */
+
+#ifndef _ARM_CONST_STRUCTS_H
+#define _ARM_CONST_STRUCTS_H
+
+#include "arm_math.h"
+#include "arm_common_tables.h"
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = {
+      16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = {
+      32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = {
+      64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = {
+      128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = {
+      256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = {
+      512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = {
+      1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = {
+      2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = {
+      4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH
+   };
+
+#endif
diff --git a/src/lib/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h
new file mode 100644
index 000000000..6f66f9ee3
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Include/arm_math.h
@@ -0,0 +1,7318 @@
+/* ----------------------------------------------------------------------
+* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+*
+* $Date:        17. January 2013
+* $Revision:    V1.4.1
+*
+* Project:      CMSIS DSP Library
+* Title:        arm_math.h
+*
+* Description:  Public header file for CMSIS DSP Library
+*
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*   - Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   - 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.
+*   - Neither the name of ARM LIMITED 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.
+ * -------------------------------------------------------------------- */
+
+/**
+   \mainpage CMSIS DSP Software Library
+   *
+   * <b>Introduction</b>
+   *
+   * This user manual describes the CMSIS DSP software library,
+   * a suite of common signal processing functions for use on Cortex-M processor based devices.
+   *
+   * The library is divided into a number of functions each covering a specific category:
+   * - Basic math functions
+   * - Fast math functions
+   * - Complex math functions
+   * - Filters
+   * - Matrix functions
+   * - Transforms
+   * - Motor control functions
+   * - Statistical functions
+   * - Support functions
+   * - Interpolation functions
+   *
+   * The library has separate functions for operating on 8-bit integers, 16-bit integers,
+   * 32-bit integer and 32-bit floating-point values.
+   *
+   * <b>Using the Library</b>
+   *
+   * The library installer contains prebuilt versions of the libraries in the <code>Lib</code> folder.
+   * - arm_cortexM4lf_math.lib (Little endian and Floating Point Unit on Cortex-M4)
+   * - arm_cortexM4bf_math.lib (Big endian and Floating Point Unit on Cortex-M4)
+   * - arm_cortexM4l_math.lib (Little endian on Cortex-M4)
+   * - arm_cortexM4b_math.lib (Big endian on Cortex-M4)
+   * - arm_cortexM3l_math.lib (Little endian on Cortex-M3)
+   * - arm_cortexM3b_math.lib (Big endian on Cortex-M3)
+   * - arm_cortexM0l_math.lib (Little endian on Cortex-M0)
+   * - arm_cortexM0b_math.lib (Big endian on Cortex-M3)
+   *
+   * The library functions are declared in the public file <code>arm_math.h</code> which is placed in the <code>Include</code> folder.
+   * Simply include this file and link the appropriate library in the application and begin calling the library functions. The Library supports single
+   * public header file <code> arm_math.h</code> for Cortex-M4/M3/M0 with little endian and big endian. Same header file will be used for floating point unit(FPU) variants.
+   * Define the appropriate pre processor MACRO ARM_MATH_CM4 or  ARM_MATH_CM3 or
+   * ARM_MATH_CM0 or ARM_MATH_CM0PLUS depending on the target processor in the application.
+   *
+   * <b>Examples</b>
+   *
+   * The library ships with a number of examples which demonstrate how to use the library functions.
+   *
+   * <b>Toolchain Support</b>
+   *
+   * The library has been developed and tested with MDK-ARM version 4.60.
+   * The library is being tested in GCC and IAR toolchains and updates on this activity will be made available shortly.
+   *
+   * <b>Building the Library</b>
+   *
+   * The library installer contains project files to re build libraries on MDK Tool chain in the <code>CMSIS\\DSP_Lib\\Source\\ARM</code> folder.
+   * - arm_cortexM0b_math.uvproj
+   * - arm_cortexM0l_math.uvproj
+   * - arm_cortexM3b_math.uvproj
+   * - arm_cortexM3l_math.uvproj
+   * - arm_cortexM4b_math.uvproj
+   * - arm_cortexM4l_math.uvproj
+   * - arm_cortexM4bf_math.uvproj
+   * - arm_cortexM4lf_math.uvproj
+   *
+   *
+   * The project can be built by opening the appropriate project in MDK-ARM 4.60 chain and defining the optional pre processor MACROs detailed above.
+   *
+   * <b>Pre-processor Macros</b>
+   *
+   * Each library project have differant pre-processor macros.
+   *
+   * - UNALIGNED_SUPPORT_DISABLE:
+   *
+   * Define macro UNALIGNED_SUPPORT_DISABLE, If the silicon does not support unaligned memory access
+   *
+   * - ARM_MATH_BIG_ENDIAN:
+   *
+   * Define macro ARM_MATH_BIG_ENDIAN to build the library for big endian targets. By default library builds for little endian targets.
+   *
+   * - ARM_MATH_MATRIX_CHECK:
+   *
+   * Define macro ARM_MATH_MATRIX_CHECK for checking on the input and output sizes of matrices
+   *
+   * - ARM_MATH_ROUNDING:
+   *
+   * Define macro ARM_MATH_ROUNDING for rounding on support functions
+   *
+   * - ARM_MATH_CMx:
+   *
+   * Define macro ARM_MATH_CM4 for building the library on Cortex-M4 target, ARM_MATH_CM3 for building library on Cortex-M3 target
+   * and ARM_MATH_CM0 for building library on cortex-M0 target, ARM_MATH_CM0PLUS for building library on cortex-M0+ target.
+   *
+   * - __FPU_PRESENT:
+   *
+   * Initialize macro __FPU_PRESENT = 1 when building on FPU supported Targets. Enable this macro for M4bf and M4lf libraries
+   *
+   * <b>Copyright Notice</b>
+   *
+   * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+   */
+
+
+/**
+ * @defgroup groupMath Basic Math Functions
+ */
+
+/**
+ * @defgroup groupFastMath Fast Math Functions
+ * This set of functions provides a fast approximation to sine, cosine, and square root.
+ * As compared to most of the other functions in the CMSIS math library, the fast math functions
+ * operate on individual values and not arrays.
+ * There are separate functions for Q15, Q31, and floating-point data.
+ *
+ */
+
+/**
+ * @defgroup groupCmplxMath Complex Math Functions
+ * This set of functions operates on complex data vectors.
+ * The data in the complex arrays is stored in an interleaved fashion
+ * (real, imag, real, imag, ...).
+ * In the API functions, the number of samples in a complex array refers
+ * to the number of complex values; the array contains twice this number of
+ * real values.
+ */
+
+/**
+ * @defgroup groupFilters Filtering Functions
+ */
+
+/**
+ * @defgroup groupMatrix Matrix Functions
+ *
+ * This set of functions provides basic matrix math operations.
+ * The functions operate on matrix data structures.  For example,
+ * the type
+ * definition for the floating-point matrix structure is shown
+ * below:
+ * <pre>
+ *     typedef struct
+ *     {
+ *       uint16_t numRows;     // number of rows of the matrix.
+ *       uint16_t numCols;     // number of columns of the matrix.
+ *       float32_t *pData;     // points to the data of the matrix.
+ *     } arm_matrix_instance_f32;
+ * </pre>
+ * There are similar definitions for Q15 and Q31 data types.
+ *
+ * The structure specifies the size of the matrix and then points to
+ * an array of data.  The array is of size <code>numRows X numCols</code>
+ * and the values are arranged in row order.  That is, the
+ * matrix element (i, j) is stored at:
+ * <pre>
+ *     pData[i*numCols + j]
+ * </pre>
+ *
+ * \par Init Functions
+ * There is an associated initialization function for each type of matrix
+ * data structure.
+ * The initialization function sets the values of the internal structure fields.
+ * Refer to the function <code>arm_mat_init_f32()</code>, <code>arm_mat_init_q31()</code>
+ * and <code>arm_mat_init_q15()</code> for floating-point, Q31 and Q15 types,  respectively.
+ *
+ * \par
+ * Use of the initialization function is optional. However, if initialization function is used
+ * then the instance structure cannot be placed into a const data section.
+ * To place the instance structure in a const data
+ * section, manually initialize the data structure.  For example:
+ * <pre>
+ * <code>arm_matrix_instance_f32 S = {nRows, nColumns, pData};</code>
+ * <code>arm_matrix_instance_q31 S = {nRows, nColumns, pData};</code>
+ * <code>arm_matrix_instance_q15 S = {nRows, nColumns, pData};</code>
+ * </pre>
+ * where <code>nRows</code> specifies the number of rows, <code>nColumns</code>
+ * specifies the number of columns, and <code>pData</code> points to the
+ * data array.
+ *
+ * \par Size Checking
+ * By default all of the matrix functions perform size checking on the input and
+ * output matrices.  For example, the matrix addition function verifies that the
+ * two input matrices and the output matrix all have the same number of rows and
+ * columns.  If the size check fails the functions return:
+ * <pre>
+ *     ARM_MATH_SIZE_MISMATCH
+ * </pre>
+ * Otherwise the functions return
+ * <pre>
+ *     ARM_MATH_SUCCESS
+ * </pre>
+ * There is some overhead associated with this matrix size checking.
+ * The matrix size checking is enabled via the \#define
+ * <pre>
+ *     ARM_MATH_MATRIX_CHECK
+ * </pre>
+ * within the library project settings.  By default this macro is defined
+ * and size checking is enabled.  By changing the project settings and
+ * undefining this macro size checking is eliminated and the functions
+ * run a bit faster.  With size checking disabled the functions always
+ * return <code>ARM_MATH_SUCCESS</code>.
+ */
+
+/**
+ * @defgroup groupTransforms Transform Functions
+ */
+
+/**
+ * @defgroup groupController Controller Functions
+ */
+
+/**
+ * @defgroup groupStats Statistics Functions
+ */
+/**
+ * @defgroup groupSupport Support Functions
+ */
+
+/**
+ * @defgroup groupInterpolation Interpolation Functions
+ * These functions perform 1- and 2-dimensional interpolation of data.
+ * Linear interpolation is used for 1-dimensional data and
+ * bilinear interpolation is used for 2-dimensional data.
+ */
+
+/**
+ * @defgroup groupExamples Examples
+ */
+#ifndef _ARM_MATH_H
+#define _ARM_MATH_H
+
+#define __CMSIS_GENERIC         /* disable NVIC and Systick functions */
+
+/* PX4 */
+#include <nuttx/config.h>
+#ifdef CONFIG_ARCH_CORTEXM4
+# define ARM_MATH_CM4 1
+#endif
+#ifdef CONFIG_ARCH_CORTEXM3
+# define ARM_MATH_CM3 1
+#endif
+#ifdef CONFIG_ARCH_FPU
+# define __FPU_PRESENT 1
+#endif
+
+#if defined (ARM_MATH_CM4)
+#include "core_cm4.h"
+#elif defined (ARM_MATH_CM3)
+#include "core_cm3.h"
+#elif defined (ARM_MATH_CM0)
+#include "core_cm0.h"
+#define ARM_MATH_CM0_FAMILY
+#elif defined (ARM_MATH_CM0PLUS)
+#include "core_cm0plus.h"
+#define ARM_MATH_CM0_FAMILY
+#else
+#include "ARMCM4.h"
+#warning "Define either ARM_MATH_CM4 OR ARM_MATH_CM3...By Default building on ARM_MATH_CM4....."
+#endif
+
+#undef  __CMSIS_GENERIC         /* enable NVIC and Systick functions */
+#include "string.h"
+#include "math.h"
+#ifdef	__cplusplus
+extern "C"
+{
+#endif
+
+
+  /**
+   * @brief Macros required for reciprocal calculation in Normalized LMS
+   */
+
+#define DELTA_Q31 			(0x100)
+#define DELTA_Q15 			0x5
+#define INDEX_MASK 			0x0000003F
+#ifndef PI
+#define PI					3.14159265358979f
+#endif
+
+  /**
+   * @brief Macros required for SINE and COSINE Fast math approximations
+   */
+
+#define TABLE_SIZE			256
+#define TABLE_SPACING_Q31	0x800000
+#define TABLE_SPACING_Q15	0x80
+
+  /**
+   * @brief Macros required for SINE and COSINE Controller functions
+   */
+  /* 1.31(q31) Fixed value of 2/360 */
+  /* -1 to +1 is divided into 360 values so total spacing is (2/360) */
+#define INPUT_SPACING			0xB60B61
+
+  /**
+   * @brief Macro for Unaligned Support
+   */
+#ifndef UNALIGNED_SUPPORT_DISABLE
+    #define ALIGN4
+#else
+  #if defined  (__GNUC__)
+    #define ALIGN4 __attribute__((aligned(4)))
+  #else
+    #define ALIGN4 __align(4)
+  #endif
+#endif	/*	#ifndef UNALIGNED_SUPPORT_DISABLE	*/
+
+  /**
+   * @brief Error status returned by some functions in the library.
+   */
+
+  typedef enum
+  {
+    ARM_MATH_SUCCESS = 0,                /**< No error */
+    ARM_MATH_ARGUMENT_ERROR = -1,        /**< One or more arguments are incorrect */
+    ARM_MATH_LENGTH_ERROR = -2,          /**< Length of data buffer is incorrect */
+    ARM_MATH_SIZE_MISMATCH = -3,         /**< Size of matrices is not compatible with the operation. */
+    ARM_MATH_NANINF = -4,                /**< Not-a-number (NaN) or infinity is generated */
+    ARM_MATH_SINGULAR = -5,              /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */
+    ARM_MATH_TEST_FAILURE = -6           /**< Test Failed  */
+  } arm_status;
+
+  /**
+   * @brief 8-bit fractional data type in 1.7 format.
+   */
+  typedef int8_t q7_t;
+
+  /**
+   * @brief 16-bit fractional data type in 1.15 format.
+   */
+  typedef int16_t q15_t;
+
+  /**
+   * @brief 32-bit fractional data type in 1.31 format.
+   */
+  typedef int32_t q31_t;
+
+  /**
+   * @brief 64-bit fractional data type in 1.63 format.
+   */
+  typedef int64_t q63_t;
+
+  /**
+   * @brief 32-bit floating-point type definition.
+   */
+  typedef float float32_t;
+
+  /**
+   * @brief 64-bit floating-point type definition.
+   */
+  typedef double float64_t;
+
+  /**
+   * @brief definition to read/write two 16 bit values.
+   */
+#if defined __CC_ARM
+#define __SIMD32_TYPE int32_t __packed
+#define CMSIS_UNUSED __attribute__((unused))
+#elif defined __ICCARM__
+#define CMSIS_UNUSED
+#define __SIMD32_TYPE int32_t __packed
+#elif defined __GNUC__
+#define __SIMD32_TYPE int32_t
+#define CMSIS_UNUSED __attribute__((unused))
+#else
+#error Unknown compiler
+#endif
+
+#define __SIMD32(addr)  (*(__SIMD32_TYPE **) & (addr))
+#define __SIMD32_CONST(addr)  ((__SIMD32_TYPE *)(addr))
+
+#define _SIMD32_OFFSET(addr)  (*(__SIMD32_TYPE *)  (addr))
+
+#define __SIMD64(addr)  (*(int64_t **) & (addr))
+
+#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)
+  /**
+   * @brief definition to pack two 16 bit values.
+   */
+#define __PKHBT(ARG1, ARG2, ARG3)      ( (((int32_t)(ARG1) <<  0) & (int32_t)0x0000FFFF) | \
+                                         (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000)  )
+#define __PKHTB(ARG1, ARG2, ARG3)      ( (((int32_t)(ARG1) <<  0) & (int32_t)0xFFFF0000) | \
+                                         (((int32_t)(ARG2) >> ARG3) & (int32_t)0x0000FFFF)  )
+
+#endif
+
+
+   /**
+   * @brief definition to pack four 8 bit values.
+   */
+#ifndef ARM_MATH_BIG_ENDIAN
+
+#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) <<  0) & (int32_t)0x000000FF) |	\
+                                (((int32_t)(v1) <<  8) & (int32_t)0x0000FF00) |	\
+							    (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) |	\
+							    (((int32_t)(v3) << 24) & (int32_t)0xFF000000)  )
+#else
+
+#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) <<  0) & (int32_t)0x000000FF) |	\
+                                (((int32_t)(v2) <<  8) & (int32_t)0x0000FF00) |	\
+							    (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) |	\
+							    (((int32_t)(v0) << 24) & (int32_t)0xFF000000)  )
+
+#endif
+
+
+  /**
+   * @brief Clips Q63 to Q31 values.
+   */
+  static __INLINE q31_t clip_q63_to_q31(
+  q63_t x)
+  {
+    return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
+      ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x;
+  }
+
+  /**
+   * @brief Clips Q63 to Q15 values.
+   */
+  static __INLINE q15_t clip_q63_to_q15(
+  q63_t x)
+  {
+    return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
+      ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15);
+  }
+
+  /**
+   * @brief Clips Q31 to Q7 values.
+   */
+  static __INLINE q7_t clip_q31_to_q7(
+  q31_t x)
+  {
+    return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ?
+      ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x;
+  }
+
+  /**
+   * @brief Clips Q31 to Q15 values.
+   */
+  static __INLINE q15_t clip_q31_to_q15(
+  q31_t x)
+  {
+    return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ?
+      ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x;
+  }
+
+  /**
+   * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format.
+   */
+
+  static __INLINE q63_t mult32x64(
+  q63_t x,
+  q31_t y)
+  {
+    return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) +
+            (((q63_t) (x >> 32) * y)));
+  }
+
+
+#if defined (ARM_MATH_CM0_FAMILY) && defined ( __CC_ARM   )
+#define __CLZ __clz
+#endif
+
+#if defined (ARM_MATH_CM0_FAMILY) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) )
+
+  static __INLINE uint32_t __CLZ(
+  q31_t data);
+
+
+  static __INLINE uint32_t __CLZ(
+  q31_t data)
+  {
+    uint32_t count = 0;
+    uint32_t mask = 0x80000000;
+
+    while((data & mask) == 0)
+    {
+      count += 1u;
+      mask = mask >> 1u;
+    }
+
+    return (count);
+
+  }
+
+#endif
+
+  /**
+   * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type.
+   */
+
+  static __INLINE uint32_t arm_recip_q31(
+  q31_t in,
+  q31_t * dst,
+  q31_t * pRecipTable)
+  {
+
+    uint32_t out, tempVal;
+    uint32_t index, i;
+    uint32_t signBits;
+
+    if(in > 0)
+    {
+      signBits = __CLZ(in) - 1;
+    }
+    else
+    {
+      signBits = __CLZ(-in) - 1;
+    }
+
+    /* Convert input sample to 1.31 format */
+    in = in << signBits;
+
+    /* calculation of index for initial approximated Val */
+    index = (uint32_t) (in >> 24u);
+    index = (index & INDEX_MASK);
+
+    /* 1.31 with exp 1 */
+    out = pRecipTable[index];
+
+    /* calculation of reciprocal value */
+    /* running approximation for two iterations */
+    for (i = 0u; i < 2u; i++)
+    {
+      tempVal = (q31_t) (((q63_t) in * out) >> 31u);
+      tempVal = 0x7FFFFFFF - tempVal;
+      /*      1.31 with exp 1 */
+      //out = (q31_t) (((q63_t) out * tempVal) >> 30u);
+      out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u);
+    }
+
+    /* write output */
+    *dst = out;
+
+    /* return num of signbits of out = 1/in value */
+    return (signBits + 1u);
+
+  }
+
+  /**
+   * @brief Function to Calculates 1/in (reciprocal) value of Q15 Data type.
+   */
+  static __INLINE uint32_t arm_recip_q15(
+  q15_t in,
+  q15_t * dst,
+  q15_t * pRecipTable)
+  {
+
+    uint32_t out = 0, tempVal = 0;
+    uint32_t index = 0, i = 0;
+    uint32_t signBits = 0;
+
+    if(in > 0)
+    {
+      signBits = __CLZ(in) - 17;
+    }
+    else
+    {
+      signBits = __CLZ(-in) - 17;
+    }
+
+    /* Convert input sample to 1.15 format */
+    in = in << signBits;
+
+    /* calculation of index for initial approximated Val */
+    index = in >> 8;
+    index = (index & INDEX_MASK);
+
+    /*      1.15 with exp 1  */
+    out = pRecipTable[index];
+
+    /* calculation of reciprocal value */
+    /* running approximation for two iterations */
+    for (i = 0; i < 2; i++)
+    {
+      tempVal = (q15_t) (((q31_t) in * out) >> 15);
+      tempVal = 0x7FFF - tempVal;
+      /*      1.15 with exp 1 */
+      out = (q15_t) (((q31_t) out * tempVal) >> 14);
+    }
+
+    /* write output */
+    *dst = out;
+
+    /* return num of signbits of out = 1/in value */
+    return (signBits + 1);
+
+  }
+
+
+  /*
+   * @brief C custom defined intrinisic function for only M0 processors
+   */
+#if defined(ARM_MATH_CM0_FAMILY)
+
+  static __INLINE q31_t __SSAT(
+  q31_t x,
+  uint32_t y)
+  {
+    int32_t posMax, negMin;
+    uint32_t i;
+
+    posMax = 1;
+    for (i = 0; i < (y - 1); i++)
+    {
+      posMax = posMax * 2;
+    }
+
+    if(x > 0)
+    {
+      posMax = (posMax - 1);
+
+      if(x > posMax)
+      {
+        x = posMax;
+      }
+    }
+    else
+    {
+      negMin = -posMax;
+
+      if(x < negMin)
+      {
+        x = negMin;
+      }
+    }
+    return (x);
+
+
+  }
+
+#endif /* end of ARM_MATH_CM0_FAMILY */
+
+
+
+  /*
+   * @brief C custom defined intrinsic function for M3 and M0 processors
+   */
+#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)
+
+  /*
+   * @brief C custom defined QADD8 for M3 and M0 processors
+   */
+  static __INLINE q31_t __QADD8(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t sum;
+    q7_t r, s, t, u;
+
+    r = (q7_t) x;
+    s = (q7_t) y;
+
+    r = __SSAT((q31_t) (r + s), 8);
+    s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8);
+    t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8);
+    u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8);
+
+    sum =
+      (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) |
+      (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF);
+
+    return sum;
+
+  }
+
+  /*
+   * @brief C custom defined QSUB8 for M3 and M0 processors
+   */
+  static __INLINE q31_t __QSUB8(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t sum;
+    q31_t r, s, t, u;
+
+    r = (q7_t) x;
+    s = (q7_t) y;
+
+    r = __SSAT((r - s), 8);
+    s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8;
+    t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16;
+    u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24;
+
+    sum =
+      (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r &
+                                                                0x000000FF);
+
+    return sum;
+  }
+
+  /*
+   * @brief C custom defined QADD16 for M3 and M0 processors
+   */
+
+  /*
+   * @brief C custom defined QADD16 for M3 and M0 processors
+   */
+  static __INLINE q31_t __QADD16(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t sum;
+    q31_t r, s;
+
+    r = (short) x;
+    s = (short) y;
+
+    r = __SSAT(r + s, 16);
+    s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16;
+
+    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+    return sum;
+
+  }
+
+  /*
+   * @brief C custom defined SHADD16 for M3 and M0 processors
+   */
+  static __INLINE q31_t __SHADD16(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t sum;
+    q31_t r, s;
+
+    r = (short) x;
+    s = (short) y;
+
+    r = ((r >> 1) + (s >> 1));
+    s = ((q31_t) ((x >> 17) + (y >> 17))) << 16;
+
+    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+    return sum;
+
+  }
+
+  /*
+   * @brief C custom defined QSUB16 for M3 and M0 processors
+   */
+  static __INLINE q31_t __QSUB16(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t sum;
+    q31_t r, s;
+
+    r = (short) x;
+    s = (short) y;
+
+    r = __SSAT(r - s, 16);
+    s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16;
+
+    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+    return sum;
+  }
+
+  /*
+   * @brief C custom defined SHSUB16 for M3 and M0 processors
+   */
+  static __INLINE q31_t __SHSUB16(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t diff;
+    q31_t r, s;
+
+    r = (short) x;
+    s = (short) y;
+
+    r = ((r >> 1) - (s >> 1));
+    s = (((x >> 17) - (y >> 17)) << 16);
+
+    diff = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+    return diff;
+  }
+
+  /*
+   * @brief C custom defined QASX for M3 and M0 processors
+   */
+  static __INLINE q31_t __QASX(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t sum = 0;
+
+    sum =
+      ((sum +
+        clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) +
+      clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16)));
+
+    return sum;
+  }
+
+  /*
+   * @brief C custom defined SHASX for M3 and M0 processors
+   */
+  static __INLINE q31_t __SHASX(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t sum;
+    q31_t r, s;
+
+    r = (short) x;
+    s = (short) y;
+
+    r = ((r >> 1) - (y >> 17));
+    s = (((x >> 17) + (s >> 1)) << 16);
+
+    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+    return sum;
+  }
+
+
+  /*
+   * @brief C custom defined QSAX for M3 and M0 processors
+   */
+  static __INLINE q31_t __QSAX(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t sum = 0;
+
+    sum =
+      ((sum +
+        clip_q31_to_q15((q31_t) ((short) (x >> 16) - (short) y))) << 16) +
+      clip_q31_to_q15((q31_t) ((short) x + (short) (y >> 16)));
+
+    return sum;
+  }
+
+  /*
+   * @brief C custom defined SHSAX for M3 and M0 processors
+   */
+  static __INLINE q31_t __SHSAX(
+  q31_t x,
+  q31_t y)
+  {
+
+    q31_t sum;
+    q31_t r, s;
+
+    r = (short) x;
+    s = (short) y;
+
+    r = ((r >> 1) + (y >> 17));
+    s = (((x >> 17) - (s >> 1)) << 16);
+
+    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+    return sum;
+  }
+
+  /*
+   * @brief C custom defined SMUSDX for M3 and M0 processors
+   */
+  static __INLINE q31_t __SMUSDX(
+  q31_t x,
+  q31_t y)
+  {
+
+    return ((q31_t) (((short) x * (short) (y >> 16)) -
+                     ((short) (x >> 16) * (short) y)));
+  }
+
+  /*
+   * @brief C custom defined SMUADX for M3 and M0 processors
+   */
+  static __INLINE q31_t __SMUADX(
+  q31_t x,
+  q31_t y)
+  {
+
+    return ((q31_t) (((short) x * (short) (y >> 16)) +
+                     ((short) (x >> 16) * (short) y)));
+  }
+
+  /*
+   * @brief C custom defined QADD for M3 and M0 processors
+   */
+  static __INLINE q31_t __QADD(
+  q31_t x,
+  q31_t y)
+  {
+    return clip_q63_to_q31((q63_t) x + y);
+  }
+
+  /*
+   * @brief C custom defined QSUB for M3 and M0 processors
+   */
+  static __INLINE q31_t __QSUB(
+  q31_t x,
+  q31_t y)
+  {
+    return clip_q63_to_q31((q63_t) x - y);
+  }
+
+  /*
+   * @brief C custom defined SMLAD for M3 and M0 processors
+   */
+  static __INLINE q31_t __SMLAD(
+  q31_t x,
+  q31_t y,
+  q31_t sum)
+  {
+
+    return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
+            ((short) x * (short) y));
+  }
+
+  /*
+   * @brief C custom defined SMLADX for M3 and M0 processors
+   */
+  static __INLINE q31_t __SMLADX(
+  q31_t x,
+  q31_t y,
+  q31_t sum)
+  {
+
+    return (sum + ((short) (x >> 16) * (short) (y)) +
+            ((short) x * (short) (y >> 16)));
+  }
+
+  /*
+   * @brief C custom defined SMLSDX for M3 and M0 processors
+   */
+  static __INLINE q31_t __SMLSDX(
+  q31_t x,
+  q31_t y,
+  q31_t sum)
+  {
+
+    return (sum - ((short) (x >> 16) * (short) (y)) +
+            ((short) x * (short) (y >> 16)));
+  }
+
+  /*
+   * @brief C custom defined SMLALD for M3 and M0 processors
+   */
+  static __INLINE q63_t __SMLALD(
+  q31_t x,
+  q31_t y,
+  q63_t sum)
+  {
+
+    return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
+            ((short) x * (short) y));
+  }
+
+  /*
+   * @brief C custom defined SMLALDX for M3 and M0 processors
+   */
+  static __INLINE q63_t __SMLALDX(
+  q31_t x,
+  q31_t y,
+  q63_t sum)
+  {
+
+    return (sum + ((short) (x >> 16) * (short) y)) +
+      ((short) x * (short) (y >> 16));
+  }
+
+  /*
+   * @brief C custom defined SMUAD for M3 and M0 processors
+   */
+  static __INLINE q31_t __SMUAD(
+  q31_t x,
+  q31_t y)
+  {
+
+    return (((x >> 16) * (y >> 16)) +
+            (((x << 16) >> 16) * ((y << 16) >> 16)));
+  }
+
+  /*
+   * @brief C custom defined SMUSD for M3 and M0 processors
+   */
+  static __INLINE q31_t __SMUSD(
+  q31_t x,
+  q31_t y)
+  {
+
+    return (-((x >> 16) * (y >> 16)) +
+            (((x << 16) >> 16) * ((y << 16) >> 16)));
+  }
+
+
+  /*
+   * @brief C custom defined SXTB16 for M3 and M0 processors
+   */
+  static __INLINE q31_t __SXTB16(
+  q31_t x)
+  {
+
+    return ((((x << 24) >> 24) & 0x0000FFFF) |
+            (((x << 8) >> 8) & 0xFFFF0000));
+  }
+
+
+#endif /* defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) */
+
+
+  /**
+   * @brief Instance structure for the Q7 FIR filter.
+   */
+  typedef struct
+  {
+    uint16_t numTaps;        /**< number of filter coefficients in the filter. */
+    q7_t *pState;            /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    q7_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps.*/
+  } arm_fir_instance_q7;
+
+  /**
+   * @brief Instance structure for the Q15 FIR filter.
+   */
+  typedef struct
+  {
+    uint16_t numTaps;         /**< number of filter coefficients in the filter. */
+    q15_t *pState;            /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    q15_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps.*/
+  } arm_fir_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q31 FIR filter.
+   */
+  typedef struct
+  {
+    uint16_t numTaps;         /**< number of filter coefficients in the filter. */
+    q31_t *pState;            /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    q31_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps. */
+  } arm_fir_instance_q31;
+
+  /**
+   * @brief Instance structure for the floating-point FIR filter.
+   */
+  typedef struct
+  {
+    uint16_t numTaps;     /**< number of filter coefficients in the filter. */
+    float32_t *pState;    /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    float32_t *pCoeffs;   /**< points to the coefficient array. The array is of length numTaps. */
+  } arm_fir_instance_f32;
+
+
+  /**
+   * @brief Processing function for the Q7 FIR filter.
+   * @param[in] *S points to an instance of the Q7 FIR filter 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.
+   * @return none.
+   */
+  void arm_fir_q7(
+  const arm_fir_instance_q7 * S,
+  q7_t * pSrc,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief  Initialization function for the Q7 FIR filter.
+   * @param[in,out] *S points to an instance of the Q7 FIR structure.
+   * @param[in] numTaps  Number of filter coefficients in the filter.
+   * @param[in] *pCoeffs points to the filter coefficients.
+   * @param[in] *pState points to the state buffer.
+   * @param[in] blockSize number of samples that are processed.
+   * @return none
+   */
+  void arm_fir_init_q7(
+  arm_fir_instance_q7 * S,
+  uint16_t numTaps,
+  q7_t * pCoeffs,
+  q7_t * pState,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Processing function for the Q15 FIR filter.
+   * @param[in] *S points to an instance of the Q15 FIR 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.
+   * @return none.
+   */
+  void arm_fir_q15(
+  const arm_fir_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the fast Q15 FIR filter for Cortex-M3 and Cortex-M4.
+   * @param[in] *S points to an instance of the Q15 FIR filter 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.
+   * @return none.
+   */
+  void arm_fir_fast_q15(
+  const arm_fir_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the Q15 FIR filter.
+   * @param[in,out] *S points to an instance of the Q15 FIR filter structure.
+   * @param[in] numTaps  Number of filter coefficients in the filter. Must be even and greater than or equal to 4.
+   * @param[in] *pCoeffs points to the filter coefficients.
+   * @param[in] *pState points to the state buffer.
+   * @param[in] blockSize number of samples that are processed at a time.
+   * @return The function returns ARM_MATH_SUCCESS if initialization was successful or ARM_MATH_ARGUMENT_ERROR if
+   * <code>numTaps</code> is not a supported value.
+   */
+
+  arm_status arm_fir_init_q15(
+  arm_fir_instance_q15 * S,
+  uint16_t numTaps,
+  q15_t * pCoeffs,
+  q15_t * pState,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the Q31 FIR filter.
+   * @param[in] *S points to an instance of the Q31 FIR filter 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.
+   * @return none.
+   */
+  void arm_fir_q31(
+  const arm_fir_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the fast Q31 FIR filter for Cortex-M3 and Cortex-M4.
+   * @param[in] *S points to an instance of the Q31 FIR 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.
+   * @return none.
+   */
+  void arm_fir_fast_q31(
+  const arm_fir_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the Q31 FIR filter.
+   * @param[in,out] *S points to an instance of the Q31 FIR structure.
+   * @param[in] 	numTaps  Number of filter coefficients in the filter.
+   * @param[in] 	*pCoeffs points to the filter coefficients.
+   * @param[in] 	*pState points to the state buffer.
+   * @param[in] 	blockSize number of samples that are processed at a time.
+   * @return 		none.
+   */
+  void arm_fir_init_q31(
+  arm_fir_instance_q31 * S,
+  uint16_t numTaps,
+  q31_t * pCoeffs,
+  q31_t * pState,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the floating-point FIR filter.
+   * @param[in] *S points to an instance of the floating-point FIR 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.
+   * @return none.
+   */
+  void arm_fir_f32(
+  const arm_fir_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the floating-point FIR filter.
+   * @param[in,out] *S points to an instance of the floating-point FIR filter structure.
+   * @param[in] 	numTaps  Number of filter coefficients in the filter.
+   * @param[in] 	*pCoeffs points to the filter coefficients.
+   * @param[in] 	*pState points to the state buffer.
+   * @param[in] 	blockSize number of samples that are processed at a time.
+   * @return    	none.
+   */
+  void arm_fir_init_f32(
+  arm_fir_instance_f32 * S,
+  uint16_t numTaps,
+  float32_t * pCoeffs,
+  float32_t * pState,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Instance structure for the Q15 Biquad cascade filter.
+   */
+  typedef struct
+  {
+    int8_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
+    q15_t *pState;            /**< Points to the array of state coefficients.  The array is of length 4*numStages. */
+    q15_t *pCoeffs;           /**< Points to the array of coefficients.  The array is of length 5*numStages. */
+    int8_t postShift;         /**< Additional shift, in bits, applied to each output sample. */
+
+  } arm_biquad_casd_df1_inst_q15;
+
+
+  /**
+   * @brief Instance structure for the Q31 Biquad cascade filter.
+   */
+  typedef struct
+  {
+    uint32_t numStages;      /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
+    q31_t *pState;           /**< Points to the array of state coefficients.  The array is of length 4*numStages. */
+    q31_t *pCoeffs;          /**< Points to the array of coefficients.  The array is of length 5*numStages. */
+    uint8_t postShift;       /**< Additional shift, in bits, applied to each output sample. */
+
+  } arm_biquad_casd_df1_inst_q31;
+
+  /**
+   * @brief Instance structure for the floating-point Biquad cascade filter.
+   */
+  typedef struct
+  {
+    uint32_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
+    float32_t *pState;          /**< Points to the array of state coefficients.  The array is of length 4*numStages. */
+    float32_t *pCoeffs;         /**< Points to the array of coefficients.  The array is of length 5*numStages. */
+
+
+  } arm_biquad_casd_df1_inst_f32;
+
+
+
+  /**
+   * @brief Processing function for the Q15 Biquad cascade filter.
+   * @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.
+   * @return     none.
+   */
+
+  void arm_biquad_cascade_df1_q15(
+  const arm_biquad_casd_df1_inst_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the Q15 Biquad cascade filter.
+   * @param[in,out] *S           points to an instance of the Q15 Biquad cascade 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 to the output. Varies according to the coefficients format
+   * @return        none
+   */
+
+  void arm_biquad_cascade_df1_init_q15(
+  arm_biquad_casd_df1_inst_q15 * S,
+  uint8_t numStages,
+  q15_t * pCoeffs,
+  q15_t * pState,
+  int8_t postShift);
+
+
+  /**
+   * @brief Fast but less precise processing function for the Q15 Biquad cascade filter for Cortex-M3 and Cortex-M4.
+   * @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.
+   * @return     none.
+   */
+
+  void arm_biquad_cascade_df1_fast_q15(
+  const arm_biquad_casd_df1_inst_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Processing function for the Q31 Biquad cascade filter
+   * @param[in]  *S         points to an instance of the Q31 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.
+   * @return     none.
+   */
+
+  void arm_biquad_cascade_df1_q31(
+  const arm_biquad_casd_df1_inst_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Fast but less precise processing function for the Q31 Biquad cascade filter for Cortex-M3 and Cortex-M4.
+   * @param[in]  *S         points to an instance of the Q31 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.
+   * @return     none.
+   */
+
+  void arm_biquad_cascade_df1_fast_q31(
+  const arm_biquad_casd_df1_inst_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the Q31 Biquad cascade filter.
+   * @param[in,out] *S           points to an instance of the Q31 Biquad cascade 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 to the output. Varies according to the coefficients format
+   * @return        none
+   */
+
+  void arm_biquad_cascade_df1_init_q31(
+  arm_biquad_casd_df1_inst_q31 * S,
+  uint8_t numStages,
+  q31_t * pCoeffs,
+  q31_t * pState,
+  int8_t postShift);
+
+  /**
+   * @brief Processing function for the floating-point Biquad cascade filter.
+   * @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.
+   * @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);
+
+  /**
+   * @brief  Initialization function for the floating-point Biquad cascade filter.
+   * @param[in,out] *S           points to an instance of the floating-point Biquad cascade 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.
+   * @return        none
+   */
+
+  void arm_biquad_cascade_df1_init_f32(
+  arm_biquad_casd_df1_inst_f32 * S,
+  uint8_t numStages,
+  float32_t * pCoeffs,
+  float32_t * pState);
+
+
+  /**
+   * @brief Instance structure for the floating-point matrix structure.
+   */
+
+  typedef struct
+  {
+    uint16_t numRows;     /**< number of rows of the matrix.     */
+    uint16_t numCols;     /**< number of columns of the matrix.  */
+    float32_t *pData;     /**< points to the data of the matrix. */
+  } arm_matrix_instance_f32;
+
+  /**
+   * @brief Instance structure for the Q15 matrix structure.
+   */
+
+  typedef struct
+  {
+    uint16_t numRows;     /**< number of rows of the matrix.     */
+    uint16_t numCols;     /**< number of columns of the matrix.  */
+    q15_t *pData;         /**< points to the data of the matrix. */
+
+  } arm_matrix_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q31 matrix structure.
+   */
+
+  typedef struct
+  {
+    uint16_t numRows;     /**< number of rows of the matrix.     */
+    uint16_t numCols;     /**< number of columns of the matrix.  */
+    q31_t *pData;         /**< points to the data of the matrix. */
+
+  } arm_matrix_instance_q31;
+
+
+
+  /**
+   * @brief Floating-point matrix addition.
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_add_f32(
+  const arm_matrix_instance_f32 * pSrcA,
+  const arm_matrix_instance_f32 * pSrcB,
+  arm_matrix_instance_f32 * pDst);
+
+  /**
+   * @brief Q15 matrix addition.
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_add_q15(
+  const arm_matrix_instance_q15 * pSrcA,
+  const arm_matrix_instance_q15 * pSrcB,
+  arm_matrix_instance_q15 * pDst);
+
+  /**
+   * @brief Q31 matrix addition.
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_add_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+  arm_matrix_instance_q31 * pDst);
+
+
+  /**
+   * @brief Floating-point matrix transpose.
+   * @param[in]  *pSrc points to the input matrix
+   * @param[out] *pDst points to the output matrix
+   * @return 	The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_trans_f32(
+  const arm_matrix_instance_f32 * pSrc,
+  arm_matrix_instance_f32 * pDst);
+
+
+  /**
+   * @brief Q15 matrix transpose.
+   * @param[in]  *pSrc points to the input matrix
+   * @param[out] *pDst points to the output matrix
+   * @return 	The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_trans_q15(
+  const arm_matrix_instance_q15 * pSrc,
+  arm_matrix_instance_q15 * pDst);
+
+  /**
+   * @brief Q31 matrix transpose.
+   * @param[in]  *pSrc points to the input matrix
+   * @param[out] *pDst points to the output matrix
+   * @return 	The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
+   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_trans_q31(
+  const arm_matrix_instance_q31 * pSrc,
+  arm_matrix_instance_q31 * pDst);
+
+
+  /**
+   * @brief Floating-point matrix multiplication
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_mult_f32(
+  const arm_matrix_instance_f32 * pSrcA,
+  const arm_matrix_instance_f32 * pSrcB,
+  arm_matrix_instance_f32 * pDst);
+
+  /**
+   * @brief Q15 matrix multiplication
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @param[in]		  *pState points to the array for storing intermediate results
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_mult_q15(
+  const arm_matrix_instance_q15 * pSrcA,
+  const arm_matrix_instance_q15 * pSrcB,
+  arm_matrix_instance_q15 * pDst,
+  q15_t * pState);
+
+  /**
+   * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
+   * @param[in]       *pSrcA  points to the first input matrix structure
+   * @param[in]       *pSrcB  points to the second input matrix structure
+   * @param[out]      *pDst   points to output matrix structure
+   * @param[in]		  *pState points to the array for storing intermediate results
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_mult_fast_q15(
+  const arm_matrix_instance_q15 * pSrcA,
+  const arm_matrix_instance_q15 * pSrcB,
+  arm_matrix_instance_q15 * pDst,
+  q15_t * pState);
+
+  /**
+   * @brief Q31 matrix multiplication
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_mult_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+  arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_mult_fast_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+  arm_matrix_instance_q31 * pDst);
+
+
+  /**
+   * @brief Floating-point matrix subtraction
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_sub_f32(
+  const arm_matrix_instance_f32 * pSrcA,
+  const arm_matrix_instance_f32 * pSrcB,
+  arm_matrix_instance_f32 * pDst);
+
+  /**
+   * @brief Q15 matrix subtraction
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_sub_q15(
+  const arm_matrix_instance_q15 * pSrcA,
+  const arm_matrix_instance_q15 * pSrcB,
+  arm_matrix_instance_q15 * pDst);
+
+  /**
+   * @brief Q31 matrix subtraction
+   * @param[in]       *pSrcA points to the first input matrix structure
+   * @param[in]       *pSrcB points to the second input matrix structure
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_sub_q31(
+  const arm_matrix_instance_q31 * pSrcA,
+  const arm_matrix_instance_q31 * pSrcB,
+  arm_matrix_instance_q31 * pDst);
+
+  /**
+   * @brief Floating-point matrix scaling.
+   * @param[in]  *pSrc points to the input matrix
+   * @param[in]  scale scale factor
+   * @param[out] *pDst points to the output matrix
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_scale_f32(
+  const arm_matrix_instance_f32 * pSrc,
+  float32_t scale,
+  arm_matrix_instance_f32 * pDst);
+
+  /**
+   * @brief Q15 matrix scaling.
+   * @param[in]       *pSrc points to input matrix
+   * @param[in]       scaleFract fractional portion of the scale factor
+   * @param[in]       shift number of bits to shift the result by
+   * @param[out]      *pDst points to output matrix
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_scale_q15(
+  const arm_matrix_instance_q15 * pSrc,
+  q15_t scaleFract,
+  int32_t shift,
+  arm_matrix_instance_q15 * pDst);
+
+  /**
+   * @brief Q31 matrix scaling.
+   * @param[in]       *pSrc points to input matrix
+   * @param[in]       scaleFract fractional portion of the scale factor
+   * @param[in]       shift number of bits to shift the result by
+   * @param[out]      *pDst points to output matrix structure
+   * @return     The function returns either
+   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+   */
+
+  arm_status arm_mat_scale_q31(
+  const arm_matrix_instance_q31 * pSrc,
+  q31_t scaleFract,
+  int32_t shift,
+  arm_matrix_instance_q31 * pDst);
+
+
+  /**
+   * @brief  Q31 matrix initialization.
+   * @param[in,out] *S             points to an instance of the floating-point matrix structure.
+   * @param[in]     nRows          number of rows in the matrix.
+   * @param[in]     nColumns       number of columns in the matrix.
+   * @param[in]     *pData	       points to the matrix data array.
+   * @return        none
+   */
+
+  void arm_mat_init_q31(
+  arm_matrix_instance_q31 * S,
+  uint16_t nRows,
+  uint16_t nColumns,
+  q31_t * pData);
+
+  /**
+   * @brief  Q15 matrix initialization.
+   * @param[in,out] *S             points to an instance of the floating-point matrix structure.
+   * @param[in]     nRows          number of rows in the matrix.
+   * @param[in]     nColumns       number of columns in the matrix.
+   * @param[in]     *pData	       points to the matrix data array.
+   * @return        none
+   */
+
+  void arm_mat_init_q15(
+  arm_matrix_instance_q15 * S,
+  uint16_t nRows,
+  uint16_t nColumns,
+  q15_t * pData);
+
+  /**
+   * @brief  Floating-point matrix initialization.
+   * @param[in,out] *S             points to an instance of the floating-point matrix structure.
+   * @param[in]     nRows          number of rows in the matrix.
+   * @param[in]     nColumns       number of columns in the matrix.
+   * @param[in]     *pData	       points to the matrix data array.
+   * @return        none
+   */
+
+  void arm_mat_init_f32(
+  arm_matrix_instance_f32 * S,
+  uint16_t nRows,
+  uint16_t nColumns,
+  float32_t * pData);
+
+
+
+  /**
+   * @brief Instance structure for the Q15 PID Control.
+   */
+  typedef struct
+  {
+    q15_t A0;    /**< The derived gain, A0 = Kp + Ki + Kd . */
+#ifdef ARM_MATH_CM0_FAMILY
+    q15_t A1;
+    q15_t A2;
+#else
+    q31_t A1;           /**< The derived gain A1 = -Kp - 2Kd | Kd.*/
+#endif
+    q15_t state[3];       /**< The state array of length 3. */
+    q15_t Kp;           /**< The proportional gain. */
+    q15_t Ki;           /**< The integral gain. */
+    q15_t Kd;           /**< The derivative gain. */
+  } arm_pid_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q31 PID Control.
+   */
+  typedef struct
+  {
+    q31_t A0;            /**< The derived gain, A0 = Kp + Ki + Kd . */
+    q31_t A1;            /**< The derived gain, A1 = -Kp - 2Kd. */
+    q31_t A2;            /**< The derived gain, A2 = Kd . */
+    q31_t state[3];      /**< The state array of length 3. */
+    q31_t Kp;            /**< The proportional gain. */
+    q31_t Ki;            /**< The integral gain. */
+    q31_t Kd;            /**< The derivative gain. */
+
+  } arm_pid_instance_q31;
+
+  /**
+   * @brief Instance structure for the floating-point PID Control.
+   */
+  typedef struct
+  {
+    float32_t A0;          /**< The derived gain, A0 = Kp + Ki + Kd . */
+    float32_t A1;          /**< The derived gain, A1 = -Kp - 2Kd. */
+    float32_t A2;          /**< The derived gain, A2 = Kd . */
+    float32_t state[3];    /**< The state array of length 3. */
+    float32_t Kp;               /**< The proportional gain. */
+    float32_t Ki;               /**< The integral gain. */
+    float32_t Kd;               /**< The derivative gain. */
+  } arm_pid_instance_f32;
+
+
+
+  /**
+   * @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.
+   */
+  void arm_pid_init_f32(
+  arm_pid_instance_f32 * S,
+  int32_t resetStateFlag);
+
+  /**
+   * @brief  Reset function for the floating-point PID Control.
+   * @param[in,out] *S is an instance of the floating-point PID Control structure
+   * @return none
+   */
+  void arm_pid_reset_f32(
+  arm_pid_instance_f32 * S);
+
+
+  /**
+   * @brief  Initialization function for the Q31 PID Control.
+   * @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.
+   */
+  void arm_pid_init_q31(
+  arm_pid_instance_q31 * S,
+  int32_t resetStateFlag);
+
+
+  /**
+   * @brief  Reset function for the Q31 PID Control.
+   * @param[in,out] *S points to an instance of the Q31 PID Control structure
+   * @return none
+   */
+
+  void arm_pid_reset_q31(
+  arm_pid_instance_q31 * S);
+
+  /**
+   * @brief  Initialization function for the Q15 PID Control.
+   * @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.
+   */
+  void arm_pid_init_q15(
+  arm_pid_instance_q15 * S,
+  int32_t resetStateFlag);
+
+  /**
+   * @brief  Reset function for the Q15 PID Control.
+   * @param[in,out] *S points to an instance of the q15 PID Control structure
+   * @return none
+   */
+  void arm_pid_reset_q15(
+  arm_pid_instance_q15 * S);
+
+
+  /**
+   * @brief Instance structure for the floating-point Linear Interpolate function.
+   */
+  typedef struct
+  {
+    uint32_t nValues;           /**< nValues */
+    float32_t x1;               /**< x1 */
+    float32_t xSpacing;         /**< xSpacing */
+    float32_t *pYData;          /**< pointer to the table of Y values */
+  } arm_linear_interp_instance_f32;
+
+  /**
+   * @brief Instance structure for the floating-point bilinear interpolation function.
+   */
+
+  typedef struct
+  {
+    uint16_t numRows;   /**< number of rows in the data table. */
+    uint16_t numCols;   /**< number of columns in the data table. */
+    float32_t *pData;   /**< points to the data table. */
+  } arm_bilinear_interp_instance_f32;
+
+   /**
+   * @brief Instance structure for the Q31 bilinear interpolation function.
+   */
+
+  typedef struct
+  {
+    uint16_t numRows;   /**< number of rows in the data table. */
+    uint16_t numCols;   /**< number of columns in the data table. */
+    q31_t *pData;       /**< points to the data table. */
+  } arm_bilinear_interp_instance_q31;
+
+   /**
+   * @brief Instance structure for the Q15 bilinear interpolation function.
+   */
+
+  typedef struct
+  {
+    uint16_t numRows;   /**< number of rows in the data table. */
+    uint16_t numCols;   /**< number of columns in the data table. */
+    q15_t *pData;       /**< points to the data table. */
+  } arm_bilinear_interp_instance_q15;
+
+   /**
+   * @brief Instance structure for the Q15 bilinear interpolation function.
+   */
+
+  typedef struct
+  {
+    uint16_t numRows;   /**< number of rows in the data table. */
+    uint16_t numCols;   /**< number of columns in the data table. */
+    q7_t *pData;                /**< points to the data table. */
+  } arm_bilinear_interp_instance_q7;
+
+
+  /**
+   * @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.
+   */
+
+  void arm_mult_q7(
+  q7_t * pSrcA,
+  q7_t * pSrcB,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_mult_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_mult_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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);
+
+
+
+
+
+
+  /**
+   * @brief Instance structure for the Q15 CFFT/CIFFT function.
+   */
+
+  typedef struct
+  {
+    uint16_t fftLen;                 /**< length of the FFT. */
+    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    q15_t *pTwiddle;                     /**< points to the Sin twiddle factor table. */
+    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
+    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+  } arm_cfft_radix2_instance_q15;
+
+  arm_status arm_cfft_radix2_init_q15(
+  arm_cfft_radix2_instance_q15 * S,
+  uint16_t fftLen,
+  uint8_t ifftFlag,
+  uint8_t bitReverseFlag);
+
+  void arm_cfft_radix2_q15(
+  const arm_cfft_radix2_instance_q15 * S,
+  q15_t * pSrc);
+
+
+
+  /**
+   * @brief Instance structure for the Q15 CFFT/CIFFT function.
+   */
+
+  typedef struct
+  {
+    uint16_t fftLen;                 /**< length of the FFT. */
+    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    q15_t *pTwiddle;                 /**< points to the twiddle factor table. */
+    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
+    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+  } arm_cfft_radix4_instance_q15;
+
+  arm_status arm_cfft_radix4_init_q15(
+  arm_cfft_radix4_instance_q15 * S,
+  uint16_t fftLen,
+  uint8_t ifftFlag,
+  uint8_t bitReverseFlag);
+
+  void arm_cfft_radix4_q15(
+  const arm_cfft_radix4_instance_q15 * S,
+  q15_t * pSrc);
+
+  /**
+   * @brief Instance structure for the Radix-2 Q31 CFFT/CIFFT function.
+   */
+
+  typedef struct
+  {
+    uint16_t fftLen;                 /**< length of the FFT. */
+    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    q31_t *pTwiddle;                     /**< points to the Twiddle factor table. */
+    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
+    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+  } arm_cfft_radix2_instance_q31;
+
+  arm_status arm_cfft_radix2_init_q31(
+  arm_cfft_radix2_instance_q31 * S,
+  uint16_t fftLen,
+  uint8_t ifftFlag,
+  uint8_t bitReverseFlag);
+
+  void arm_cfft_radix2_q31(
+  const arm_cfft_radix2_instance_q31 * S,
+  q31_t * pSrc);
+
+  /**
+   * @brief Instance structure for the Q31 CFFT/CIFFT function.
+   */
+
+  typedef struct
+  {
+    uint16_t fftLen;                 /**< length of the FFT. */
+    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    q31_t *pTwiddle;                 /**< points to the twiddle factor table. */
+    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
+    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+  } arm_cfft_radix4_instance_q31;
+
+
+  void arm_cfft_radix4_q31(
+  const arm_cfft_radix4_instance_q31 * S,
+  q31_t * pSrc);
+
+  arm_status arm_cfft_radix4_init_q31(
+  arm_cfft_radix4_instance_q31 * S,
+  uint16_t fftLen,
+  uint8_t ifftFlag,
+  uint8_t bitReverseFlag);
+
+  /**
+   * @brief Instance structure for the floating-point CFFT/CIFFT function.
+   */
+
+  typedef struct
+  {
+    uint16_t fftLen;                   /**< length of the FFT. */
+    uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+    uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    float32_t *pTwiddle;               /**< points to the Twiddle factor table. */
+    uint16_t *pBitRevTable;            /**< points to the bit reversal table. */
+    uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+    float32_t onebyfftLen;                 /**< value of 1/fftLen. */
+  } arm_cfft_radix2_instance_f32;
+
+/* Deprecated */
+  arm_status arm_cfft_radix2_init_f32(
+  arm_cfft_radix2_instance_f32 * S,
+  uint16_t fftLen,
+  uint8_t ifftFlag,
+  uint8_t bitReverseFlag);
+
+/* Deprecated */
+  void arm_cfft_radix2_f32(
+  const arm_cfft_radix2_instance_f32 * S,
+  float32_t * pSrc);
+
+  /**
+   * @brief Instance structure for the floating-point CFFT/CIFFT function.
+   */
+
+  typedef struct
+  {
+    uint16_t fftLen;                   /**< length of the FFT. */
+    uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+    uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+    float32_t *pTwiddle;               /**< points to the Twiddle factor table. */
+    uint16_t *pBitRevTable;            /**< points to the bit reversal table. */
+    uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+    float32_t onebyfftLen;                 /**< value of 1/fftLen. */
+  } arm_cfft_radix4_instance_f32;
+
+/* Deprecated */
+  arm_status arm_cfft_radix4_init_f32(
+  arm_cfft_radix4_instance_f32 * S,
+  uint16_t fftLen,
+  uint8_t ifftFlag,
+  uint8_t bitReverseFlag);
+
+/* Deprecated */
+  void arm_cfft_radix4_f32(
+  const arm_cfft_radix4_instance_f32 * S,
+  float32_t * pSrc);
+
+  /**
+   * @brief Instance structure for the floating-point CFFT/CIFFT function.
+   */
+
+  typedef struct
+  {
+    uint16_t fftLen;                   /**< length of the FFT. */
+    const float32_t *pTwiddle;         /**< points to the Twiddle factor table. */
+    const uint16_t *pBitRevTable;      /**< points to the bit reversal table. */
+    uint16_t bitRevLength;             /**< bit reversal table length. */
+  } arm_cfft_instance_f32;
+
+  void arm_cfft_f32(
+  const arm_cfft_instance_f32 * S,
+  float32_t * p1,
+  uint8_t ifftFlag,
+  uint8_t bitReverseFlag);
+
+  /**
+   * @brief Instance structure for the Q15 RFFT/RIFFT function.
+   */
+
+  typedef struct
+  {
+    uint32_t fftLenReal;                      /**< length of the real FFT. */
+    uint32_t fftLenBy2;                       /**< length of the complex FFT. */
+    uint8_t ifftFlagR;                        /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
+    uint8_t bitReverseFlagR;                      /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
+    uint32_t twidCoefRModifier;               /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    q15_t *pTwiddleAReal;                     /**< points to the real twiddle factor table. */
+    q15_t *pTwiddleBReal;                     /**< points to the imag twiddle factor table. */
+    arm_cfft_radix4_instance_q15 *pCfft;          /**< points to the complex FFT instance. */
+  } arm_rfft_instance_q15;
+
+  arm_status arm_rfft_init_q15(
+  arm_rfft_instance_q15 * S,
+  arm_cfft_radix4_instance_q15 * S_CFFT,
+  uint32_t fftLenReal,
+  uint32_t ifftFlagR,
+  uint32_t bitReverseFlag);
+
+  void arm_rfft_q15(
+  const arm_rfft_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst);
+
+  /**
+   * @brief Instance structure for the Q31 RFFT/RIFFT function.
+   */
+
+  typedef struct
+  {
+    uint32_t fftLenReal;                        /**< length of the real FFT. */
+    uint32_t fftLenBy2;                         /**< length of the complex FFT. */
+    uint8_t ifftFlagR;                          /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
+    uint8_t bitReverseFlagR;                        /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
+    uint32_t twidCoefRModifier;                 /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    q31_t *pTwiddleAReal;                       /**< points to the real twiddle factor table. */
+    q31_t *pTwiddleBReal;                       /**< points to the imag twiddle factor table. */
+    arm_cfft_radix4_instance_q31 *pCfft;        /**< points to the complex FFT instance. */
+  } arm_rfft_instance_q31;
+
+  arm_status arm_rfft_init_q31(
+  arm_rfft_instance_q31 * S,
+  arm_cfft_radix4_instance_q31 * S_CFFT,
+  uint32_t fftLenReal,
+  uint32_t ifftFlagR,
+  uint32_t bitReverseFlag);
+
+  void arm_rfft_q31(
+  const arm_rfft_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst);
+
+  /**
+   * @brief Instance structure for the floating-point RFFT/RIFFT function.
+   */
+
+  typedef struct
+  {
+    uint32_t fftLenReal;                        /**< length of the real FFT. */
+    uint16_t fftLenBy2;                         /**< length of the complex FFT. */
+    uint8_t ifftFlagR;                          /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
+    uint8_t bitReverseFlagR;                    /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
+    uint32_t twidCoefRModifier;                     /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+    float32_t *pTwiddleAReal;                   /**< points to the real twiddle factor table. */
+    float32_t *pTwiddleBReal;                   /**< points to the imag twiddle factor table. */
+    arm_cfft_radix4_instance_f32 *pCfft;        /**< points to the complex FFT instance. */
+  } arm_rfft_instance_f32;
+
+  arm_status arm_rfft_init_f32(
+  arm_rfft_instance_f32 * S,
+  arm_cfft_radix4_instance_f32 * S_CFFT,
+  uint32_t fftLenReal,
+  uint32_t ifftFlagR,
+  uint32_t bitReverseFlag);
+
+  void arm_rfft_f32(
+  const arm_rfft_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pDst);
+
+  /**
+   * @brief Instance structure for the floating-point RFFT/RIFFT function.
+   */
+
+typedef struct
+  {
+    arm_cfft_instance_f32 Sint;      /**< Internal CFFT structure. */
+    uint16_t fftLenRFFT;                        /**< length of the real sequence */
+	float32_t * pTwiddleRFFT;					/**< Twiddle factors real stage  */
+  } arm_rfft_fast_instance_f32 ;
+
+arm_status arm_rfft_fast_init_f32 (
+	arm_rfft_fast_instance_f32 * S,
+	uint16_t fftLen);
+
+void arm_rfft_fast_f32(
+  arm_rfft_fast_instance_f32 * S,
+  float32_t * p, float32_t * pOut,
+  uint8_t ifftFlag);
+
+  /**
+   * @brief Instance structure for the floating-point DCT4/IDCT4 function.
+   */
+
+  typedef struct
+  {
+    uint16_t N;                         /**< length of the DCT4. */
+    uint16_t Nby2;                      /**< half of the length of the DCT4. */
+    float32_t normalize;                /**< normalizing factor. */
+    float32_t *pTwiddle;                /**< points to the twiddle factor table. */
+    float32_t *pCosFactor;              /**< points to the cosFactor table. */
+    arm_rfft_instance_f32 *pRfft;        /**< points to the real FFT instance. */
+    arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */
+  } arm_dct4_instance_f32;
+
+  /**
+   * @brief  Initialization function for the floating-point DCT4/IDCT4.
+   * @param[in,out] *S         points to an instance of floating-point DCT4/IDCT4 structure.
+   * @param[in]     *S_RFFT    points to an instance of floating-point RFFT/RIFFT structure.
+   * @param[in]     *S_CFFT    points to an instance of floating-point CFFT/CIFFT structure.
+   * @param[in]     N          length of the DCT4.
+   * @param[in]     Nby2       half of the length of the DCT4.
+   * @param[in]     normalize  normalizing factor.
+   * @return		arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>fftLenReal</code> is not a supported transform length.
+   */
+
+  arm_status arm_dct4_init_f32(
+  arm_dct4_instance_f32 * S,
+  arm_rfft_instance_f32 * S_RFFT,
+  arm_cfft_radix4_instance_f32 * S_CFFT,
+  uint16_t N,
+  uint16_t Nby2,
+  float32_t normalize);
+
+  /**
+   * @brief Processing function for the floating-point DCT4/IDCT4.
+   * @param[in]       *S             points to an instance of the floating-point DCT4/IDCT4 structure.
+   * @param[in]       *pState        points to state buffer.
+   * @param[in,out]   *pInlineBuffer points to the in-place input and output buffer.
+   * @return none.
+   */
+
+  void arm_dct4_f32(
+  const arm_dct4_instance_f32 * S,
+  float32_t * pState,
+  float32_t * pInlineBuffer);
+
+  /**
+   * @brief Instance structure for the Q31 DCT4/IDCT4 function.
+   */
+
+  typedef struct
+  {
+    uint16_t N;                         /**< length of the DCT4. */
+    uint16_t Nby2;                      /**< half of the length of the DCT4. */
+    q31_t normalize;                    /**< normalizing factor. */
+    q31_t *pTwiddle;                    /**< points to the twiddle factor table. */
+    q31_t *pCosFactor;                  /**< points to the cosFactor table. */
+    arm_rfft_instance_q31 *pRfft;        /**< points to the real FFT instance. */
+    arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */
+  } arm_dct4_instance_q31;
+
+  /**
+   * @brief  Initialization function for the Q31 DCT4/IDCT4.
+   * @param[in,out] *S         points to an instance of Q31 DCT4/IDCT4 structure.
+   * @param[in]     *S_RFFT    points to an instance of Q31 RFFT/RIFFT structure
+   * @param[in]     *S_CFFT    points to an instance of Q31 CFFT/CIFFT structure
+   * @param[in]     N          length of the DCT4.
+   * @param[in]     Nby2       half of the length of the DCT4.
+   * @param[in]     normalize  normalizing factor.
+   * @return		arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
+   */
+
+  arm_status arm_dct4_init_q31(
+  arm_dct4_instance_q31 * S,
+  arm_rfft_instance_q31 * S_RFFT,
+  arm_cfft_radix4_instance_q31 * S_CFFT,
+  uint16_t N,
+  uint16_t Nby2,
+  q31_t normalize);
+
+  /**
+   * @brief Processing function for the Q31 DCT4/IDCT4.
+   * @param[in]       *S             points to an instance of the Q31 DCT4 structure.
+   * @param[in]       *pState        points to state buffer.
+   * @param[in,out]   *pInlineBuffer points to the in-place input and output buffer.
+   * @return none.
+   */
+
+  void arm_dct4_q31(
+  const arm_dct4_instance_q31 * S,
+  q31_t * pState,
+  q31_t * pInlineBuffer);
+
+  /**
+   * @brief Instance structure for the Q15 DCT4/IDCT4 function.
+   */
+
+  typedef struct
+  {
+    uint16_t N;                         /**< length of the DCT4. */
+    uint16_t Nby2;                      /**< half of the length of the DCT4. */
+    q15_t normalize;                    /**< normalizing factor. */
+    q15_t *pTwiddle;                    /**< points to the twiddle factor table. */
+    q15_t *pCosFactor;                  /**< points to the cosFactor table. */
+    arm_rfft_instance_q15 *pRfft;        /**< points to the real FFT instance. */
+    arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */
+  } arm_dct4_instance_q15;
+
+  /**
+   * @brief  Initialization function for the Q15 DCT4/IDCT4.
+   * @param[in,out] *S         points to an instance of Q15 DCT4/IDCT4 structure.
+   * @param[in]     *S_RFFT    points to an instance of Q15 RFFT/RIFFT structure.
+   * @param[in]     *S_CFFT    points to an instance of Q15 CFFT/CIFFT structure.
+   * @param[in]     N          length of the DCT4.
+   * @param[in]     Nby2       half of the length of the DCT4.
+   * @param[in]     normalize  normalizing factor.
+   * @return		arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
+   */
+
+  arm_status arm_dct4_init_q15(
+  arm_dct4_instance_q15 * S,
+  arm_rfft_instance_q15 * S_RFFT,
+  arm_cfft_radix4_instance_q15 * S_CFFT,
+  uint16_t N,
+  uint16_t Nby2,
+  q15_t normalize);
+
+  /**
+   * @brief Processing function for the Q15 DCT4/IDCT4.
+   * @param[in]       *S             points to an instance of the Q15 DCT4 structure.
+   * @param[in]       *pState        points to state buffer.
+   * @param[in,out]   *pInlineBuffer points to the in-place input and output buffer.
+   * @return none.
+   */
+
+  void arm_dct4_q15(
+  const arm_dct4_instance_q15 * S,
+  q15_t * pState,
+  q15_t * pInlineBuffer);
+
+  /**
+   * @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);
+
+  /**
+   * @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.
+   */
+
+  void arm_add_q7(
+  q7_t * pSrcA,
+  q7_t * pSrcB,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_add_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_add_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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);
+
+  /**
+   * @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.
+   */
+
+  void arm_sub_q7(
+  q7_t * pSrcA,
+  q7_t * pSrcB,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_sub_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_sub_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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);
+
+  /**
+   * @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.
+   */
+
+  void arm_scale_q7(
+  q7_t * pSrc,
+  q7_t scaleFract,
+  int8_t shift,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_scale_q15(
+  q15_t * pSrc,
+  q15_t scaleFract,
+  int8_t shift,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_scale_q31(
+  q31_t * pSrc,
+  q31_t scaleFract,
+  int8_t shift,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_abs_q7(
+  q7_t * pSrc,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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);
+
+  /**
+   * @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.
+   */
+
+  void arm_abs_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_abs_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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);
+
+  /**
+   * @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.
+   */
+
+  void arm_dot_prod_q7(
+  q7_t * pSrcA,
+  q7_t * pSrcB,
+  uint32_t blockSize,
+  q31_t * result);
+
+  /**
+   * @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.
+   */
+
+  void arm_dot_prod_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  uint32_t blockSize,
+  q63_t * result);
+
+  /**
+   * @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.
+   */
+
+  void arm_dot_prod_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  uint32_t blockSize,
+  q63_t * result);
+
+  /**
+   * @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.
+   */
+
+  void arm_shift_q7(
+  q7_t * pSrc,
+  int8_t shiftBits,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_shift_q15(
+  q15_t * pSrc,
+  int8_t shiftBits,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_shift_q31(
+  q31_t * pSrc,
+  int8_t shiftBits,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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);
+
+  /**
+   * @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.
+   */
+
+  void arm_offset_q7(
+  q7_t * pSrc,
+  q7_t offset,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_offset_q15(
+  q15_t * pSrc,
+  q15_t offset,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_offset_q31(
+  q31_t * pSrc,
+  q31_t offset,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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);
+
+  /**
+   * @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.
+   */
+
+  void arm_negate_q7(
+  q7_t * pSrc,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_negate_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @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.
+   */
+
+  void arm_negate_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+  /**
+   * @brief  Copies the elements of a floating-point vector.
+   * @param[in]  *pSrc input pointer
+   * @param[out]  *pDst output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_copy_f32(
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Copies the elements of a Q7 vector.
+   * @param[in]  *pSrc input pointer
+   * @param[out]  *pDst output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_copy_q7(
+  q7_t * pSrc,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Copies the elements of a Q15 vector.
+   * @param[in]  *pSrc input pointer
+   * @param[out]  *pDst output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_copy_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Copies the elements of a Q31 vector.
+   * @param[in]  *pSrc input pointer
+   * @param[out]  *pDst output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_copy_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+  /**
+   * @brief  Fills a constant value into a floating-point vector.
+   * @param[in]  value input value to be filled
+   * @param[out]  *pDst output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_fill_f32(
+  float32_t value,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Fills a constant value into a Q7 vector.
+   * @param[in]  value input value to be filled
+   * @param[out]  *pDst output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_fill_q7(
+  q7_t value,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Fills a constant value into a Q15 vector.
+   * @param[in]  value input value to be filled
+   * @param[out]  *pDst output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_fill_q15(
+  q15_t value,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Fills a constant value into a Q31 vector.
+   * @param[in]  value input value to be filled
+   * @param[out]  *pDst output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_fill_q31(
+  q31_t value,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+/**
+ * @brief Convolution of floating-point sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the location where the output result is written.  Length srcALen+srcBLen-1.
+ * @return none.
+ */
+
+  void arm_conv_f32(
+  float32_t * pSrcA,
+  uint32_t srcALen,
+  float32_t * pSrcB,
+  uint32_t srcBLen,
+  float32_t * pDst);
+
+
+  /**
+   * @brief Convolution of Q15 sequences.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
+   * @param[in]  *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+   * @param[in]  *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+   * @return none.
+   */
+
+
+  void arm_conv_opt_q15(
+  q15_t * pSrcA,
+  uint32_t srcALen,
+  q15_t * pSrcB,
+  uint32_t srcBLen,
+  q15_t * pDst,
+  q15_t * pScratch1,
+  q15_t * pScratch2);
+
+
+/**
+ * @brief Convolution of Q15 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the location where the output result is written.  Length srcALen+srcBLen-1.
+ * @return none.
+ */
+
+  void arm_conv_q15(
+  q15_t * pSrcA,
+  uint32_t srcALen,
+  q15_t * pSrcB,
+  uint32_t srcBLen,
+  q15_t * pDst);
+
+  /**
+   * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
+   * @return none.
+   */
+
+  void arm_conv_fast_q15(
+			  q15_t * pSrcA,
+			 uint32_t srcALen,
+			  q15_t * pSrcB,
+			 uint32_t srcBLen,
+			 q15_t * pDst);
+
+  /**
+   * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
+   * @param[in]  *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+   * @param[in]  *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+   * @return none.
+   */
+
+  void arm_conv_fast_opt_q15(
+  q15_t * pSrcA,
+  uint32_t srcALen,
+  q15_t * pSrcB,
+  uint32_t srcBLen,
+  q15_t * pDst,
+  q15_t * pScratch1,
+  q15_t * pScratch2);
+
+
+
+  /**
+   * @brief Convolution of Q31 sequences.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
+   * @return none.
+   */
+
+  void arm_conv_q31(
+  q31_t * pSrcA,
+  uint32_t srcALen,
+  q31_t * pSrcB,
+  uint32_t srcBLen,
+  q31_t * pDst);
+
+  /**
+   * @brief Convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
+   * @return none.
+   */
+
+  void arm_conv_fast_q31(
+  q31_t * pSrcA,
+  uint32_t srcALen,
+  q31_t * pSrcB,
+  uint32_t srcBLen,
+  q31_t * pDst);
+
+
+    /**
+   * @brief Convolution of Q7 sequences.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
+   * @param[in]  *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+   * @param[in]  *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
+   * @return none.
+   */
+
+  void arm_conv_opt_q7(
+  q7_t * pSrcA,
+  uint32_t srcALen,
+  q7_t * pSrcB,
+  uint32_t srcBLen,
+  q7_t * pDst,
+  q15_t * pScratch1,
+  q15_t * pScratch2);
+
+
+
+  /**
+   * @brief Convolution of Q7 sequences.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
+   * @return none.
+   */
+
+  void arm_conv_q7(
+  q7_t * pSrcA,
+  uint32_t srcALen,
+  q7_t * pSrcB,
+  uint32_t srcBLen,
+  q7_t * pDst);
+
+
+  /**
+   * @brief Partial convolution of floating-point sequences.
+   * @param[in]       *pSrcA points to the first input sequence.
+   * @param[in]       srcALen length of the first input sequence.
+   * @param[in]       *pSrcB points to the second input sequence.
+   * @param[in]       srcBLen length of the second input sequence.
+   * @param[out]      *pDst points to the block of output data
+   * @param[in]       firstIndex is the first output sample to start with.
+   * @param[in]       numPoints is the number of output points to be computed.
+   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+   */
+
+  arm_status arm_conv_partial_f32(
+  float32_t * pSrcA,
+  uint32_t srcALen,
+  float32_t * pSrcB,
+  uint32_t srcBLen,
+  float32_t * pDst,
+  uint32_t firstIndex,
+  uint32_t numPoints);
+
+    /**
+   * @brief Partial convolution of Q15 sequences.
+   * @param[in]       *pSrcA points to the first input sequence.
+   * @param[in]       srcALen length of the first input sequence.
+   * @param[in]       *pSrcB points to the second input sequence.
+   * @param[in]       srcBLen length of the second input sequence.
+   * @param[out]      *pDst points to the block of output data
+   * @param[in]       firstIndex is the first output sample to start with.
+   * @param[in]       numPoints is the number of output points to be computed.
+   * @param[in]       * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+   * @param[in]       * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+   */
+
+  arm_status arm_conv_partial_opt_q15(
+  q15_t * pSrcA,
+  uint32_t srcALen,
+  q15_t * pSrcB,
+  uint32_t srcBLen,
+  q15_t * pDst,
+  uint32_t firstIndex,
+  uint32_t numPoints,
+  q15_t * pScratch1,
+  q15_t * pScratch2);
+
+
+/**
+   * @brief Partial convolution of Q15 sequences.
+   * @param[in]       *pSrcA points to the first input sequence.
+   * @param[in]       srcALen length of the first input sequence.
+   * @param[in]       *pSrcB points to the second input sequence.
+   * @param[in]       srcBLen length of the second input sequence.
+   * @param[out]      *pDst points to the block of output data
+   * @param[in]       firstIndex is the first output sample to start with.
+   * @param[in]       numPoints is the number of output points to be computed.
+   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+   */
+
+  arm_status arm_conv_partial_q15(
+  q15_t * pSrcA,
+  uint32_t srcALen,
+  q15_t * pSrcB,
+  uint32_t srcBLen,
+  q15_t * pDst,
+  uint32_t firstIndex,
+  uint32_t numPoints);
+
+  /**
+   * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
+   * @param[in]       *pSrcA points to the first input sequence.
+   * @param[in]       srcALen length of the first input sequence.
+   * @param[in]       *pSrcB points to the second input sequence.
+   * @param[in]       srcBLen length of the second input sequence.
+   * @param[out]      *pDst points to the block of output data
+   * @param[in]       firstIndex is the first output sample to start with.
+   * @param[in]       numPoints is the number of output points to be computed.
+   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+   */
+
+  arm_status arm_conv_partial_fast_q15(
+				        q15_t * pSrcA,
+				       uint32_t srcALen,
+				        q15_t * pSrcB,
+				       uint32_t srcBLen,
+				       q15_t * pDst,
+				       uint32_t firstIndex,
+				       uint32_t numPoints);
+
+
+  /**
+   * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
+   * @param[in]       *pSrcA points to the first input sequence.
+   * @param[in]       srcALen length of the first input sequence.
+   * @param[in]       *pSrcB points to the second input sequence.
+   * @param[in]       srcBLen length of the second input sequence.
+   * @param[out]      *pDst points to the block of output data
+   * @param[in]       firstIndex is the first output sample to start with.
+   * @param[in]       numPoints is the number of output points to be computed.
+   * @param[in]       * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+   * @param[in]       * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+   */
+
+  arm_status arm_conv_partial_fast_opt_q15(
+  q15_t * pSrcA,
+  uint32_t srcALen,
+  q15_t * pSrcB,
+  uint32_t srcBLen,
+  q15_t * pDst,
+  uint32_t firstIndex,
+  uint32_t numPoints,
+  q15_t * pScratch1,
+  q15_t * pScratch2);
+
+
+  /**
+   * @brief Partial convolution of Q31 sequences.
+   * @param[in]       *pSrcA points to the first input sequence.
+   * @param[in]       srcALen length of the first input sequence.
+   * @param[in]       *pSrcB points to the second input sequence.
+   * @param[in]       srcBLen length of the second input sequence.
+   * @param[out]      *pDst points to the block of output data
+   * @param[in]       firstIndex is the first output sample to start with.
+   * @param[in]       numPoints is the number of output points to be computed.
+   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+   */
+
+  arm_status arm_conv_partial_q31(
+  q31_t * pSrcA,
+  uint32_t srcALen,
+  q31_t * pSrcB,
+  uint32_t srcBLen,
+  q31_t * pDst,
+  uint32_t firstIndex,
+  uint32_t numPoints);
+
+
+  /**
+   * @brief Partial convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
+   * @param[in]       *pSrcA points to the first input sequence.
+   * @param[in]       srcALen length of the first input sequence.
+   * @param[in]       *pSrcB points to the second input sequence.
+   * @param[in]       srcBLen length of the second input sequence.
+   * @param[out]      *pDst points to the block of output data
+   * @param[in]       firstIndex is the first output sample to start with.
+   * @param[in]       numPoints is the number of output points to be computed.
+   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+   */
+
+  arm_status arm_conv_partial_fast_q31(
+  q31_t * pSrcA,
+  uint32_t srcALen,
+  q31_t * pSrcB,
+  uint32_t srcBLen,
+  q31_t * pDst,
+  uint32_t firstIndex,
+  uint32_t numPoints);
+
+
+  /**
+   * @brief Partial convolution of Q7 sequences
+   * @param[in]       *pSrcA points to the first input sequence.
+   * @param[in]       srcALen length of the first input sequence.
+   * @param[in]       *pSrcB points to the second input sequence.
+   * @param[in]       srcBLen length of the second input sequence.
+   * @param[out]      *pDst points to the block of output data
+   * @param[in]       firstIndex is the first output sample to start with.
+   * @param[in]       numPoints is the number of output points to be computed.
+   * @param[in]  *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+   * @param[in]  *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
+   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+   */
+
+  arm_status arm_conv_partial_opt_q7(
+  q7_t * pSrcA,
+  uint32_t srcALen,
+  q7_t * pSrcB,
+  uint32_t srcBLen,
+  q7_t * pDst,
+  uint32_t firstIndex,
+  uint32_t numPoints,
+  q15_t * pScratch1,
+  q15_t * pScratch2);
+
+
+/**
+   * @brief Partial convolution of Q7 sequences.
+   * @param[in]       *pSrcA points to the first input sequence.
+   * @param[in]       srcALen length of the first input sequence.
+   * @param[in]       *pSrcB points to the second input sequence.
+   * @param[in]       srcBLen length of the second input sequence.
+   * @param[out]      *pDst points to the block of output data
+   * @param[in]       firstIndex is the first output sample to start with.
+   * @param[in]       numPoints is the number of output points to be computed.
+   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+   */
+
+  arm_status arm_conv_partial_q7(
+  q7_t * pSrcA,
+  uint32_t srcALen,
+  q7_t * pSrcB,
+  uint32_t srcBLen,
+  q7_t * pDst,
+  uint32_t firstIndex,
+  uint32_t numPoints);
+
+
+
+  /**
+   * @brief Instance structure for the Q15 FIR decimator.
+   */
+
+  typedef struct
+  {
+    uint8_t M;                      /**< decimation factor. */
+    uint16_t numTaps;               /**< number of coefficients in the filter. */
+    q15_t *pCoeffs;                  /**< points to the coefficient array. The array is of length numTaps.*/
+    q15_t *pState;                   /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+  } arm_fir_decimate_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q31 FIR decimator.
+   */
+
+  typedef struct
+  {
+    uint8_t M;                  /**< decimation factor. */
+    uint16_t numTaps;           /**< number of coefficients in the filter. */
+    q31_t *pCoeffs;              /**< points to the coefficient array. The array is of length numTaps.*/
+    q31_t *pState;               /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+
+  } arm_fir_decimate_instance_q31;
+
+  /**
+   * @brief Instance structure for the floating-point FIR decimator.
+   */
+
+  typedef struct
+  {
+    uint8_t M;                          /**< decimation factor. */
+    uint16_t numTaps;                   /**< number of coefficients in the filter. */
+    float32_t *pCoeffs;                  /**< points to the coefficient array. The array is of length numTaps.*/
+    float32_t *pState;                   /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+
+  } arm_fir_decimate_instance_f32;
+
+
+
+  /**
+   * @brief Processing function for the floating-point FIR decimator.
+   * @param[in] *S points to an instance of the floating-point FIR decimator 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 input samples to process per call.
+   * @return none
+   */
+
+  void arm_fir_decimate_f32(
+  const arm_fir_decimate_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief  Initialization function for the floating-point FIR decimator.
+   * @param[in,out] *S points to an instance of the floating-point FIR decimator structure.
+   * @param[in] numTaps  number of coefficients in the filter.
+   * @param[in] M  decimation factor.
+   * @param[in] *pCoeffs points to the filter coefficients.
+   * @param[in] *pState points to the state buffer.
+   * @param[in] blockSize number of input samples to process per call.
+   * @return    The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+   * <code>blockSize</code> is not a multiple of <code>M</code>.
+   */
+
+  arm_status arm_fir_decimate_init_f32(
+  arm_fir_decimate_instance_f32 * S,
+  uint16_t numTaps,
+  uint8_t M,
+  float32_t * pCoeffs,
+  float32_t * pState,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the Q15 FIR decimator.
+   * @param[in] *S points to an instance of the Q15 FIR decimator 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 input samples to process per call.
+   * @return none
+   */
+
+  void arm_fir_decimate_q15(
+  const arm_fir_decimate_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
+   * @param[in] *S points to an instance of the Q15 FIR decimator 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 input samples to process per call.
+   * @return none
+   */
+
+  void arm_fir_decimate_fast_q15(
+  const arm_fir_decimate_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+
+
+  /**
+   * @brief  Initialization function for the Q15 FIR decimator.
+   * @param[in,out] *S points to an instance of the Q15 FIR decimator structure.
+   * @param[in] numTaps  number of coefficients in the filter.
+   * @param[in] M  decimation factor.
+   * @param[in] *pCoeffs points to the filter coefficients.
+   * @param[in] *pState points to the state buffer.
+   * @param[in] blockSize number of input samples to process per call.
+   * @return    The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+   * <code>blockSize</code> is not a multiple of <code>M</code>.
+   */
+
+  arm_status arm_fir_decimate_init_q15(
+  arm_fir_decimate_instance_q15 * S,
+  uint16_t numTaps,
+  uint8_t M,
+  q15_t * pCoeffs,
+  q15_t * pState,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the Q31 FIR decimator.
+   * @param[in] *S points to an instance of the Q31 FIR decimator 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 input samples to process per call.
+   * @return none
+   */
+
+  void arm_fir_decimate_q31(
+  const arm_fir_decimate_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the Q31 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
+   * @param[in] *S points to an instance of the Q31 FIR decimator 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 input samples to process per call.
+   * @return none
+   */
+
+  void arm_fir_decimate_fast_q31(
+  arm_fir_decimate_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief  Initialization function for the Q31 FIR decimator.
+   * @param[in,out] *S points to an instance of the Q31 FIR decimator structure.
+   * @param[in] numTaps  number of coefficients in the filter.
+   * @param[in] M  decimation factor.
+   * @param[in] *pCoeffs points to the filter coefficients.
+   * @param[in] *pState points to the state buffer.
+   * @param[in] blockSize number of input samples to process per call.
+   * @return    The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+   * <code>blockSize</code> is not a multiple of <code>M</code>.
+   */
+
+  arm_status arm_fir_decimate_init_q31(
+  arm_fir_decimate_instance_q31 * S,
+  uint16_t numTaps,
+  uint8_t M,
+  q31_t * pCoeffs,
+  q31_t * pState,
+  uint32_t blockSize);
+
+
+
+  /**
+   * @brief Instance structure for the Q15 FIR interpolator.
+   */
+
+  typedef struct
+  {
+    uint8_t L;                      /**< upsample factor. */
+    uint16_t phaseLength;           /**< length of each polyphase filter component. */
+    q15_t *pCoeffs;                 /**< points to the coefficient array. The array is of length L*phaseLength. */
+    q15_t *pState;                  /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
+  } arm_fir_interpolate_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q31 FIR interpolator.
+   */
+
+  typedef struct
+  {
+    uint8_t L;                      /**< upsample factor. */
+    uint16_t phaseLength;           /**< length of each polyphase filter component. */
+    q31_t *pCoeffs;                  /**< points to the coefficient array. The array is of length L*phaseLength. */
+    q31_t *pState;                   /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
+  } arm_fir_interpolate_instance_q31;
+
+  /**
+   * @brief Instance structure for the floating-point FIR interpolator.
+   */
+
+  typedef struct
+  {
+    uint8_t L;                     /**< upsample factor. */
+    uint16_t phaseLength;          /**< length of each polyphase filter component. */
+    float32_t *pCoeffs;             /**< points to the coefficient array. The array is of length L*phaseLength. */
+    float32_t *pState;              /**< points to the state variable array. The array is of length phaseLength+numTaps-1. */
+  } arm_fir_interpolate_instance_f32;
+
+
+  /**
+   * @brief Processing function for the Q15 FIR interpolator.
+   * @param[in] *S        points to an instance of the Q15 FIR interpolator 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 input samples to process per call.
+   * @return none.
+   */
+
+  void arm_fir_interpolate_q15(
+  const arm_fir_interpolate_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief  Initialization function for the Q15 FIR interpolator.
+   * @param[in,out] *S        points to an instance of the Q15 FIR interpolator structure.
+   * @param[in]     L         upsample factor.
+   * @param[in]     numTaps   number of filter coefficients in the filter.
+   * @param[in]     *pCoeffs  points to the filter coefficient buffer.
+   * @param[in]     *pState   points to the state buffer.
+   * @param[in]     blockSize number of input samples to process per call.
+   * @return        The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+   * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
+   */
+
+  arm_status arm_fir_interpolate_init_q15(
+  arm_fir_interpolate_instance_q15 * S,
+  uint8_t L,
+  uint16_t numTaps,
+  q15_t * pCoeffs,
+  q15_t * pState,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the Q31 FIR interpolator.
+   * @param[in] *S        points to an instance of the Q15 FIR interpolator 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 input samples to process per call.
+   * @return none.
+   */
+
+  void arm_fir_interpolate_q31(
+  const arm_fir_interpolate_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the Q31 FIR interpolator.
+   * @param[in,out] *S        points to an instance of the Q31 FIR interpolator structure.
+   * @param[in]     L         upsample factor.
+   * @param[in]     numTaps   number of filter coefficients in the filter.
+   * @param[in]     *pCoeffs  points to the filter coefficient buffer.
+   * @param[in]     *pState   points to the state buffer.
+   * @param[in]     blockSize number of input samples to process per call.
+   * @return        The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+   * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
+   */
+
+  arm_status arm_fir_interpolate_init_q31(
+  arm_fir_interpolate_instance_q31 * S,
+  uint8_t L,
+  uint16_t numTaps,
+  q31_t * pCoeffs,
+  q31_t * pState,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Processing function for the floating-point FIR interpolator.
+   * @param[in] *S        points to an instance of the floating-point FIR interpolator 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 input samples to process per call.
+   * @return none.
+   */
+
+  void arm_fir_interpolate_f32(
+  const arm_fir_interpolate_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the floating-point FIR interpolator.
+   * @param[in,out] *S        points to an instance of the floating-point FIR interpolator structure.
+   * @param[in]     L         upsample factor.
+   * @param[in]     numTaps   number of filter coefficients in the filter.
+   * @param[in]     *pCoeffs  points to the filter coefficient buffer.
+   * @param[in]     *pState   points to the state buffer.
+   * @param[in]     blockSize number of input samples to process per call.
+   * @return        The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+   * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
+   */
+
+  arm_status arm_fir_interpolate_init_f32(
+  arm_fir_interpolate_instance_f32 * S,
+  uint8_t L,
+  uint16_t numTaps,
+  float32_t * pCoeffs,
+  float32_t * pState,
+  uint32_t blockSize);
+
+  /**
+   * @brief Instance structure for the high precision Q31 Biquad cascade filter.
+   */
+
+  typedef struct
+  {
+    uint8_t numStages;       /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
+    q63_t *pState;           /**< points to the array of state coefficients.  The array is of length 4*numStages. */
+    q31_t *pCoeffs;          /**< points to the array of coefficients.  The array is of length 5*numStages. */
+    uint8_t postShift;       /**< additional shift, in bits, applied to each output sample. */
+
+  } arm_biquad_cas_df1_32x64_ins_q31;
+
+
+  /**
+   * @param[in]  *S        points to an instance of the high precision Q31 Biquad cascade filter 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.
+   * @return none.
+   */
+
+  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);
+
+
+  /**
+   * @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 to the output. Varies according to the coefficients format
+   * @return        none
+   */
+
+  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);
+
+
+
+  /**
+   * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter.
+   */
+
+  typedef struct
+  {
+    uint8_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
+    float32_t *pState;         /**< points to the array of state coefficients.  The array is of length 2*numStages. */
+    float32_t *pCoeffs;        /**< points to the array of coefficients.  The array is of length 5*numStages. */
+  } arm_biquad_cascade_df2T_instance_f32;
+
+
+  /**
+   * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
+   * @param[in]  *S        points to an instance of the filter data 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.
+   * @return none.
+   */
+
+  void arm_biquad_cascade_df2T_f32(
+  const arm_biquad_cascade_df2T_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief  Initialization function for the floating-point transposed direct form II Biquad cascade filter.
+   * @param[in,out] *S           points to an instance of the filter data 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.
+   * @return        none
+   */
+
+  void arm_biquad_cascade_df2T_init_f32(
+  arm_biquad_cascade_df2T_instance_f32 * S,
+  uint8_t numStages,
+  float32_t * pCoeffs,
+  float32_t * pState);
+
+
+
+  /**
+   * @brief Instance structure for the Q15 FIR lattice filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numStages;                          /**< number of filter stages. */
+    q15_t *pState;                               /**< points to the state variable array. The array is of length numStages. */
+    q15_t *pCoeffs;                              /**< points to the coefficient array. The array is of length numStages. */
+  } arm_fir_lattice_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q31 FIR lattice filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numStages;                          /**< number of filter stages. */
+    q31_t *pState;                               /**< points to the state variable array. The array is of length numStages. */
+    q31_t *pCoeffs;                              /**< points to the coefficient array. The array is of length numStages. */
+  } arm_fir_lattice_instance_q31;
+
+  /**
+   * @brief Instance structure for the floating-point FIR lattice filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numStages;                  /**< number of filter stages. */
+    float32_t *pState;                   /**< points to the state variable array. The array is of length numStages. */
+    float32_t *pCoeffs;                  /**< points to the coefficient array. The array is of length numStages. */
+  } arm_fir_lattice_instance_f32;
+
+  /**
+   * @brief Initialization function for the Q15 FIR lattice filter.
+   * @param[in] *S points to an instance of the Q15 FIR lattice structure.
+   * @param[in] numStages  number of filter stages.
+   * @param[in] *pCoeffs points to the coefficient buffer.  The array is of length numStages.
+   * @param[in] *pState points to the state buffer.  The array is of length numStages.
+   * @return none.
+   */
+
+  void arm_fir_lattice_init_q15(
+  arm_fir_lattice_instance_q15 * S,
+  uint16_t numStages,
+  q15_t * pCoeffs,
+  q15_t * pState);
+
+
+  /**
+   * @brief Processing function for the Q15 FIR lattice filter.
+   * @param[in] *S points to an instance of the Q15 FIR lattice 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.
+   * @return none.
+   */
+  void arm_fir_lattice_q15(
+  const arm_fir_lattice_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Initialization function for the Q31 FIR lattice filter.
+   * @param[in] *S points to an instance of the Q31 FIR lattice structure.
+   * @param[in] numStages  number of filter stages.
+   * @param[in] *pCoeffs points to the coefficient buffer.  The array is of length numStages.
+   * @param[in] *pState points to the state buffer.   The array is of length numStages.
+   * @return none.
+   */
+
+  void arm_fir_lattice_init_q31(
+  arm_fir_lattice_instance_q31 * S,
+  uint16_t numStages,
+  q31_t * pCoeffs,
+  q31_t * pState);
+
+
+  /**
+   * @brief Processing function for the Q31 FIR lattice filter.
+   * @param[in]  *S        points to an instance of the Q31 FIR lattice 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.
+   * @return none.
+   */
+
+  void arm_fir_lattice_q31(
+  const arm_fir_lattice_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+/**
+ * @brief Initialization function for the floating-point FIR lattice filter.
+ * @param[in] *S points to an instance of the floating-point FIR lattice structure.
+ * @param[in] numStages  number of filter stages.
+ * @param[in] *pCoeffs points to the coefficient buffer.  The array is of length numStages.
+ * @param[in] *pState points to the state buffer.  The array is of length numStages.
+ * @return none.
+ */
+
+  void arm_fir_lattice_init_f32(
+  arm_fir_lattice_instance_f32 * S,
+  uint16_t numStages,
+  float32_t * pCoeffs,
+  float32_t * pState);
+
+  /**
+   * @brief Processing function for the floating-point FIR lattice filter.
+   * @param[in]  *S        points to an instance of the floating-point FIR lattice 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.
+   * @return none.
+   */
+
+  void arm_fir_lattice_f32(
+  const arm_fir_lattice_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Instance structure for the Q15 IIR lattice filter.
+   */
+  typedef struct
+  {
+    uint16_t numStages;                         /**< number of stages in the filter. */
+    q15_t *pState;                              /**< points to the state variable array. The array is of length numStages+blockSize. */
+    q15_t *pkCoeffs;                            /**< points to the reflection coefficient array. The array is of length numStages. */
+    q15_t *pvCoeffs;                            /**< points to the ladder coefficient array. The array is of length numStages+1. */
+  } arm_iir_lattice_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q31 IIR lattice filter.
+   */
+  typedef struct
+  {
+    uint16_t numStages;                         /**< number of stages in the filter. */
+    q31_t *pState;                              /**< points to the state variable array. The array is of length numStages+blockSize. */
+    q31_t *pkCoeffs;                            /**< points to the reflection coefficient array. The array is of length numStages. */
+    q31_t *pvCoeffs;                            /**< points to the ladder coefficient array. The array is of length numStages+1. */
+  } arm_iir_lattice_instance_q31;
+
+  /**
+   * @brief Instance structure for the floating-point IIR lattice filter.
+   */
+  typedef struct
+  {
+    uint16_t numStages;                         /**< number of stages in the filter. */
+    float32_t *pState;                          /**< points to the state variable array. The array is of length numStages+blockSize. */
+    float32_t *pkCoeffs;                        /**< points to the reflection coefficient array. The array is of length numStages. */
+    float32_t *pvCoeffs;                        /**< points to the ladder coefficient array. The array is of length numStages+1. */
+  } arm_iir_lattice_instance_f32;
+
+  /**
+   * @brief Processing function for the floating-point IIR lattice filter.
+   * @param[in] *S points to an instance of the floating-point IIR lattice 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.
+   * @return none.
+   */
+
+  void arm_iir_lattice_f32(
+  const arm_iir_lattice_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Initialization function for the floating-point IIR lattice filter.
+   * @param[in] *S points to an instance of the floating-point IIR lattice structure.
+   * @param[in] numStages number of stages in the filter.
+   * @param[in] *pkCoeffs points to the reflection coefficient buffer.  The array is of length numStages.
+   * @param[in] *pvCoeffs points to the ladder coefficient buffer.  The array is of length numStages+1.
+   * @param[in] *pState points to the state buffer.  The array is of length numStages+blockSize-1.
+   * @param[in] blockSize number of samples to process.
+   * @return none.
+   */
+
+  void arm_iir_lattice_init_f32(
+  arm_iir_lattice_instance_f32 * S,
+  uint16_t numStages,
+  float32_t * pkCoeffs,
+  float32_t * pvCoeffs,
+  float32_t * pState,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Processing function for the Q31 IIR lattice filter.
+   * @param[in] *S points to an instance of the Q31 IIR lattice 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.
+   * @return none.
+   */
+
+  void arm_iir_lattice_q31(
+  const arm_iir_lattice_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Initialization function for the Q31 IIR lattice filter.
+   * @param[in] *S points to an instance of the Q31 IIR lattice structure.
+   * @param[in] numStages number of stages in the filter.
+   * @param[in] *pkCoeffs points to the reflection coefficient buffer.  The array is of length numStages.
+   * @param[in] *pvCoeffs points to the ladder coefficient buffer.  The array is of length numStages+1.
+   * @param[in] *pState points to the state buffer.  The array is of length numStages+blockSize.
+   * @param[in] blockSize number of samples to process.
+   * @return none.
+   */
+
+  void arm_iir_lattice_init_q31(
+  arm_iir_lattice_instance_q31 * S,
+  uint16_t numStages,
+  q31_t * pkCoeffs,
+  q31_t * pvCoeffs,
+  q31_t * pState,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Processing function for the Q15 IIR lattice filter.
+   * @param[in] *S points to an instance of the Q15 IIR lattice 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.
+   * @return none.
+   */
+
+  void arm_iir_lattice_q15(
+  const arm_iir_lattice_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+
+/**
+ * @brief Initialization function for the Q15 IIR lattice filter.
+ * @param[in] *S points to an instance of the fixed-point Q15 IIR lattice structure.
+ * @param[in] numStages  number of stages in the filter.
+ * @param[in] *pkCoeffs points to reflection coefficient buffer.  The array is of length numStages.
+ * @param[in] *pvCoeffs points to ladder coefficient buffer.  The array is of length numStages+1.
+ * @param[in] *pState points to state buffer.  The array is of length numStages+blockSize.
+ * @param[in] blockSize number of samples to process per call.
+ * @return none.
+ */
+
+  void arm_iir_lattice_init_q15(
+  arm_iir_lattice_instance_q15 * S,
+  uint16_t numStages,
+  q15_t * pkCoeffs,
+  q15_t * pvCoeffs,
+  q15_t * pState,
+  uint32_t blockSize);
+
+  /**
+   * @brief Instance structure for the floating-point LMS filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numTaps;    /**< number of coefficients in the filter. */
+    float32_t *pState;   /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    float32_t *pCoeffs;  /**< points to the coefficient array. The array is of length numTaps. */
+    float32_t mu;        /**< step size that controls filter coefficient updates. */
+  } arm_lms_instance_f32;
+
+  /**
+   * @brief Processing function for floating-point LMS filter.
+   * @param[in]  *S points to an instance of the floating-point LMS filter structure.
+   * @param[in]  *pSrc points to the block of input data.
+   * @param[in]  *pRef points to the block of reference data.
+   * @param[out] *pOut points to the block of output data.
+   * @param[out] *pErr points to the block of error data.
+   * @param[in]  blockSize number of samples to process.
+   * @return     none.
+   */
+
+  void arm_lms_f32(
+  const arm_lms_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pRef,
+  float32_t * pOut,
+  float32_t * pErr,
+  uint32_t blockSize);
+
+  /**
+   * @brief Initialization function for floating-point LMS filter.
+   * @param[in] *S points to an instance of the floating-point LMS filter structure.
+   * @param[in] numTaps  number of filter coefficients.
+   * @param[in] *pCoeffs points to the coefficient buffer.
+   * @param[in] *pState points to state buffer.
+   * @param[in] mu step size that controls filter coefficient updates.
+   * @param[in] blockSize number of samples to process.
+   * @return none.
+   */
+
+  void arm_lms_init_f32(
+  arm_lms_instance_f32 * S,
+  uint16_t numTaps,
+  float32_t * pCoeffs,
+  float32_t * pState,
+  float32_t mu,
+  uint32_t blockSize);
+
+  /**
+   * @brief Instance structure for the Q15 LMS filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numTaps;    /**< number of coefficients in the filter. */
+    q15_t *pState;       /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    q15_t *pCoeffs;      /**< points to the coefficient array. The array is of length numTaps. */
+    q15_t mu;            /**< step size that controls filter coefficient updates. */
+    uint32_t postShift;  /**< bit shift applied to coefficients. */
+  } arm_lms_instance_q15;
+
+
+  /**
+   * @brief Initialization function for the Q15 LMS filter.
+   * @param[in] *S points to an instance of the Q15 LMS filter structure.
+   * @param[in] numTaps  number of filter coefficients.
+   * @param[in] *pCoeffs points to the coefficient buffer.
+   * @param[in] *pState points to the state buffer.
+   * @param[in] mu step size that controls filter coefficient updates.
+   * @param[in] blockSize number of samples to process.
+   * @param[in] postShift bit shift applied to coefficients.
+   * @return    none.
+   */
+
+  void arm_lms_init_q15(
+  arm_lms_instance_q15 * S,
+  uint16_t numTaps,
+  q15_t * pCoeffs,
+  q15_t * pState,
+  q15_t mu,
+  uint32_t blockSize,
+  uint32_t postShift);
+
+  /**
+   * @brief Processing function for Q15 LMS filter.
+   * @param[in] *S points to an instance of the Q15 LMS filter structure.
+   * @param[in] *pSrc points to the block of input data.
+   * @param[in] *pRef points to the block of reference data.
+   * @param[out] *pOut points to the block of output data.
+   * @param[out] *pErr points to the block of error data.
+   * @param[in] blockSize number of samples to process.
+   * @return none.
+   */
+
+  void arm_lms_q15(
+  const arm_lms_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pRef,
+  q15_t * pOut,
+  q15_t * pErr,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Instance structure for the Q31 LMS filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numTaps;    /**< number of coefficients in the filter. */
+    q31_t *pState;       /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    q31_t *pCoeffs;      /**< points to the coefficient array. The array is of length numTaps. */
+    q31_t mu;            /**< step size that controls filter coefficient updates. */
+    uint32_t postShift;  /**< bit shift applied to coefficients. */
+
+  } arm_lms_instance_q31;
+
+  /**
+   * @brief Processing function for Q31 LMS filter.
+   * @param[in]  *S points to an instance of the Q15 LMS filter structure.
+   * @param[in]  *pSrc points to the block of input data.
+   * @param[in]  *pRef points to the block of reference data.
+   * @param[out] *pOut points to the block of output data.
+   * @param[out] *pErr points to the block of error data.
+   * @param[in]  blockSize number of samples to process.
+   * @return     none.
+   */
+
+  void arm_lms_q31(
+  const arm_lms_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pRef,
+  q31_t * pOut,
+  q31_t * pErr,
+  uint32_t blockSize);
+
+  /**
+   * @brief Initialization function for Q31 LMS filter.
+   * @param[in] *S points to an instance of the Q31 LMS filter structure.
+   * @param[in] numTaps  number of filter coefficients.
+   * @param[in] *pCoeffs points to coefficient buffer.
+   * @param[in] *pState points to state buffer.
+   * @param[in] mu step size that controls filter coefficient updates.
+   * @param[in] blockSize number of samples to process.
+   * @param[in] postShift bit shift applied to coefficients.
+   * @return none.
+   */
+
+  void arm_lms_init_q31(
+  arm_lms_instance_q31 * S,
+  uint16_t numTaps,
+  q31_t * pCoeffs,
+  q31_t * pState,
+  q31_t mu,
+  uint32_t blockSize,
+  uint32_t postShift);
+
+  /**
+   * @brief Instance structure for the floating-point normalized LMS filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numTaps;     /**< number of coefficients in the filter. */
+    float32_t *pState;    /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    float32_t *pCoeffs;   /**< points to the coefficient array. The array is of length numTaps. */
+    float32_t mu;        /**< step size that control filter coefficient updates. */
+    float32_t energy;    /**< saves previous frame energy. */
+    float32_t x0;        /**< saves previous input sample. */
+  } arm_lms_norm_instance_f32;
+
+  /**
+   * @brief Processing function for floating-point normalized LMS filter.
+   * @param[in] *S points to an instance of the floating-point normalized LMS filter structure.
+   * @param[in] *pSrc points to the block of input data.
+   * @param[in] *pRef points to the block of reference data.
+   * @param[out] *pOut points to the block of output data.
+   * @param[out] *pErr points to the block of error data.
+   * @param[in] blockSize number of samples to process.
+   * @return none.
+   */
+
+  void arm_lms_norm_f32(
+  arm_lms_norm_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pRef,
+  float32_t * pOut,
+  float32_t * pErr,
+  uint32_t blockSize);
+
+  /**
+   * @brief Initialization function for floating-point normalized LMS filter.
+   * @param[in] *S points to an instance of the floating-point LMS filter structure.
+   * @param[in] numTaps  number of filter coefficients.
+   * @param[in] *pCoeffs points to coefficient buffer.
+   * @param[in] *pState points to state buffer.
+   * @param[in] mu step size that controls filter coefficient updates.
+   * @param[in] blockSize number of samples to process.
+   * @return none.
+   */
+
+  void arm_lms_norm_init_f32(
+  arm_lms_norm_instance_f32 * S,
+  uint16_t numTaps,
+  float32_t * pCoeffs,
+  float32_t * pState,
+  float32_t mu,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Instance structure for the Q31 normalized LMS filter.
+   */
+  typedef struct
+  {
+    uint16_t numTaps;     /**< number of coefficients in the filter. */
+    q31_t *pState;        /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    q31_t *pCoeffs;       /**< points to the coefficient array. The array is of length numTaps. */
+    q31_t mu;             /**< step size that controls filter coefficient updates. */
+    uint8_t postShift;    /**< bit shift applied to coefficients. */
+    q31_t *recipTable;    /**< points to the reciprocal initial value table. */
+    q31_t energy;         /**< saves previous frame energy. */
+    q31_t x0;             /**< saves previous input sample. */
+  } arm_lms_norm_instance_q31;
+
+  /**
+   * @brief Processing function for Q31 normalized LMS filter.
+   * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
+   * @param[in] *pSrc points to the block of input data.
+   * @param[in] *pRef points to the block of reference data.
+   * @param[out] *pOut points to the block of output data.
+   * @param[out] *pErr points to the block of error data.
+   * @param[in] blockSize number of samples to process.
+   * @return none.
+   */
+
+  void arm_lms_norm_q31(
+  arm_lms_norm_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pRef,
+  q31_t * pOut,
+  q31_t * pErr,
+  uint32_t blockSize);
+
+  /**
+   * @brief Initialization function for Q31 normalized LMS filter.
+   * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
+   * @param[in] numTaps  number of filter coefficients.
+   * @param[in] *pCoeffs points to coefficient buffer.
+   * @param[in] *pState points to state buffer.
+   * @param[in] mu step size that controls filter coefficient updates.
+   * @param[in] blockSize number of samples to process.
+   * @param[in] postShift bit shift applied to coefficients.
+   * @return none.
+   */
+
+  void arm_lms_norm_init_q31(
+  arm_lms_norm_instance_q31 * S,
+  uint16_t numTaps,
+  q31_t * pCoeffs,
+  q31_t * pState,
+  q31_t mu,
+  uint32_t blockSize,
+  uint8_t postShift);
+
+  /**
+   * @brief Instance structure for the Q15 normalized LMS filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numTaps;    /**< Number of coefficients in the filter. */
+    q15_t *pState;        /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+    q15_t *pCoeffs;       /**< points to the coefficient array. The array is of length numTaps. */
+    q15_t mu;            /**< step size that controls filter coefficient updates. */
+    uint8_t postShift;   /**< bit shift applied to coefficients. */
+    q15_t *recipTable;   /**< Points to the reciprocal initial value table. */
+    q15_t energy;        /**< saves previous frame energy. */
+    q15_t x0;            /**< saves previous input sample. */
+  } arm_lms_norm_instance_q15;
+
+  /**
+   * @brief Processing function for Q15 normalized LMS filter.
+   * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
+   * @param[in] *pSrc points to the block of input data.
+   * @param[in] *pRef points to the block of reference data.
+   * @param[out] *pOut points to the block of output data.
+   * @param[out] *pErr points to the block of error data.
+   * @param[in] blockSize number of samples to process.
+   * @return none.
+   */
+
+  void arm_lms_norm_q15(
+  arm_lms_norm_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pRef,
+  q15_t * pOut,
+  q15_t * pErr,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief Initialization function for Q15 normalized LMS filter.
+   * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
+   * @param[in] numTaps  number of filter coefficients.
+   * @param[in] *pCoeffs points to coefficient buffer.
+   * @param[in] *pState points to state buffer.
+   * @param[in] mu step size that controls filter coefficient updates.
+   * @param[in] blockSize number of samples to process.
+   * @param[in] postShift bit shift applied to coefficients.
+   * @return none.
+   */
+
+  void arm_lms_norm_init_q15(
+  arm_lms_norm_instance_q15 * S,
+  uint16_t numTaps,
+  q15_t * pCoeffs,
+  q15_t * pState,
+  q15_t mu,
+  uint32_t blockSize,
+  uint8_t postShift);
+
+  /**
+   * @brief Correlation of floating-point sequences.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   * @return none.
+   */
+
+  void arm_correlate_f32(
+  float32_t * pSrcA,
+  uint32_t srcALen,
+  float32_t * pSrcB,
+  uint32_t srcBLen,
+  float32_t * pDst);
+
+
+   /**
+   * @brief Correlation of Q15 sequences
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   * @param[in]  *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+   * @return none.
+   */
+  void arm_correlate_opt_q15(
+  q15_t * pSrcA,
+  uint32_t srcALen,
+  q15_t * pSrcB,
+  uint32_t srcBLen,
+  q15_t * pDst,
+  q15_t * pScratch);
+
+
+  /**
+   * @brief Correlation of Q15 sequences.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   * @return none.
+   */
+
+  void arm_correlate_q15(
+  q15_t * pSrcA,
+  uint32_t srcALen,
+  q15_t * pSrcB,
+  uint32_t srcBLen,
+  q15_t * pDst);
+
+  /**
+   * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   * @return none.
+   */
+
+  void arm_correlate_fast_q15(
+			       q15_t * pSrcA,
+			      uint32_t srcALen,
+			       q15_t * pSrcB,
+			      uint32_t srcBLen,
+			      q15_t * pDst);
+
+
+
+  /**
+   * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   * @param[in]  *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+   * @return none.
+   */
+
+  void arm_correlate_fast_opt_q15(
+  q15_t * pSrcA,
+  uint32_t srcALen,
+  q15_t * pSrcB,
+  uint32_t srcBLen,
+  q15_t * pDst,
+  q15_t * pScratch);
+
+  /**
+   * @brief Correlation of Q31 sequences.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   * @return none.
+   */
+
+  void arm_correlate_q31(
+  q31_t * pSrcA,
+  uint32_t srcALen,
+  q31_t * pSrcB,
+  uint32_t srcBLen,
+  q31_t * pDst);
+
+  /**
+   * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   * @return none.
+   */
+
+  void arm_correlate_fast_q31(
+  q31_t * pSrcA,
+  uint32_t srcALen,
+  q31_t * pSrcB,
+  uint32_t srcBLen,
+  q31_t * pDst);
+
+
+
+ /**
+   * @brief Correlation of Q7 sequences.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   * @param[in]  *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+   * @param[in]  *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
+   * @return none.
+   */
+
+  void arm_correlate_opt_q7(
+  q7_t * pSrcA,
+  uint32_t srcALen,
+  q7_t * pSrcB,
+  uint32_t srcBLen,
+  q7_t * pDst,
+  q15_t * pScratch1,
+  q15_t * pScratch2);
+
+
+  /**
+   * @brief Correlation of Q7 sequences.
+   * @param[in] *pSrcA points to the first input sequence.
+   * @param[in] srcALen length of the first input sequence.
+   * @param[in] *pSrcB points to the second input sequence.
+   * @param[in] srcBLen length of the second input sequence.
+   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
+   * @return none.
+   */
+
+  void arm_correlate_q7(
+  q7_t * pSrcA,
+  uint32_t srcALen,
+  q7_t * pSrcB,
+  uint32_t srcBLen,
+  q7_t * pDst);
+
+
+  /**
+   * @brief Instance structure for the floating-point sparse FIR filter.
+   */
+  typedef struct
+  {
+    uint16_t numTaps;             /**< number of coefficients in the filter. */
+    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */
+    float32_t *pState;            /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
+    float32_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps.*/
+    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */
+    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */
+  } arm_fir_sparse_instance_f32;
+
+  /**
+   * @brief Instance structure for the Q31 sparse FIR filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numTaps;             /**< number of coefficients in the filter. */
+    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */
+    q31_t *pState;                /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
+    q31_t *pCoeffs;               /**< points to the coefficient array. The array is of length numTaps.*/
+    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */
+    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */
+  } arm_fir_sparse_instance_q31;
+
+  /**
+   * @brief Instance structure for the Q15 sparse FIR filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numTaps;             /**< number of coefficients in the filter. */
+    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */
+    q15_t *pState;                /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
+    q15_t *pCoeffs;               /**< points to the coefficient array. The array is of length numTaps.*/
+    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */
+    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */
+  } arm_fir_sparse_instance_q15;
+
+  /**
+   * @brief Instance structure for the Q7 sparse FIR filter.
+   */
+
+  typedef struct
+  {
+    uint16_t numTaps;             /**< number of coefficients in the filter. */
+    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */
+    q7_t *pState;                 /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
+    q7_t *pCoeffs;                /**< points to the coefficient array. The array is of length numTaps.*/
+    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */
+    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */
+  } arm_fir_sparse_instance_q7;
+
+  /**
+   * @brief Processing function for the floating-point sparse FIR filter.
+   * @param[in]  *S          points to an instance of the floating-point sparse FIR structure.
+   * @param[in]  *pSrc       points to the block of input data.
+   * @param[out] *pDst       points to the block of output data
+   * @param[in]  *pScratchIn points to a temporary buffer of size blockSize.
+   * @param[in]  blockSize   number of input samples to process per call.
+   * @return none.
+   */
+
+  void arm_fir_sparse_f32(
+  arm_fir_sparse_instance_f32 * S,
+  float32_t * pSrc,
+  float32_t * pDst,
+  float32_t * pScratchIn,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the floating-point sparse FIR filter.
+   * @param[in,out] *S         points to an instance of the floating-point sparse FIR structure.
+   * @param[in]     numTaps    number of nonzero coefficients in the filter.
+   * @param[in]     *pCoeffs   points to the array of filter coefficients.
+   * @param[in]     *pState    points to the state buffer.
+   * @param[in]     *pTapDelay points to the array of offset times.
+   * @param[in]     maxDelay   maximum offset time supported.
+   * @param[in]     blockSize  number of samples that will be processed per block.
+   * @return none
+   */
+
+  void arm_fir_sparse_init_f32(
+  arm_fir_sparse_instance_f32 * S,
+  uint16_t numTaps,
+  float32_t * pCoeffs,
+  float32_t * pState,
+  int32_t * pTapDelay,
+  uint16_t maxDelay,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the Q31 sparse FIR filter.
+   * @param[in]  *S          points to an instance of the Q31 sparse FIR structure.
+   * @param[in]  *pSrc       points to the block of input data.
+   * @param[out] *pDst       points to the block of output data
+   * @param[in]  *pScratchIn points to a temporary buffer of size blockSize.
+   * @param[in]  blockSize   number of input samples to process per call.
+   * @return none.
+   */
+
+  void arm_fir_sparse_q31(
+  arm_fir_sparse_instance_q31 * S,
+  q31_t * pSrc,
+  q31_t * pDst,
+  q31_t * pScratchIn,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the Q31 sparse FIR filter.
+   * @param[in,out] *S         points to an instance of the Q31 sparse FIR structure.
+   * @param[in]     numTaps    number of nonzero coefficients in the filter.
+   * @param[in]     *pCoeffs   points to the array of filter coefficients.
+   * @param[in]     *pState    points to the state buffer.
+   * @param[in]     *pTapDelay points to the array of offset times.
+   * @param[in]     maxDelay   maximum offset time supported.
+   * @param[in]     blockSize  number of samples that will be processed per block.
+   * @return none
+   */
+
+  void arm_fir_sparse_init_q31(
+  arm_fir_sparse_instance_q31 * S,
+  uint16_t numTaps,
+  q31_t * pCoeffs,
+  q31_t * pState,
+  int32_t * pTapDelay,
+  uint16_t maxDelay,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the Q15 sparse FIR filter.
+   * @param[in]  *S           points to an instance of the Q15 sparse FIR structure.
+   * @param[in]  *pSrc        points to the block of input data.
+   * @param[out] *pDst        points to the block of output data
+   * @param[in]  *pScratchIn  points to a temporary buffer of size blockSize.
+   * @param[in]  *pScratchOut points to a temporary buffer of size blockSize.
+   * @param[in]  blockSize    number of input samples to process per call.
+   * @return none.
+   */
+
+  void arm_fir_sparse_q15(
+  arm_fir_sparse_instance_q15 * S,
+  q15_t * pSrc,
+  q15_t * pDst,
+  q15_t * pScratchIn,
+  q31_t * pScratchOut,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief  Initialization function for the Q15 sparse FIR filter.
+   * @param[in,out] *S         points to an instance of the Q15 sparse FIR structure.
+   * @param[in]     numTaps    number of nonzero coefficients in the filter.
+   * @param[in]     *pCoeffs   points to the array of filter coefficients.
+   * @param[in]     *pState    points to the state buffer.
+   * @param[in]     *pTapDelay points to the array of offset times.
+   * @param[in]     maxDelay   maximum offset time supported.
+   * @param[in]     blockSize  number of samples that will be processed per block.
+   * @return none
+   */
+
+  void arm_fir_sparse_init_q15(
+  arm_fir_sparse_instance_q15 * S,
+  uint16_t numTaps,
+  q15_t * pCoeffs,
+  q15_t * pState,
+  int32_t * pTapDelay,
+  uint16_t maxDelay,
+  uint32_t blockSize);
+
+  /**
+   * @brief Processing function for the Q7 sparse FIR filter.
+   * @param[in]  *S           points to an instance of the Q7 sparse FIR structure.
+   * @param[in]  *pSrc        points to the block of input data.
+   * @param[out] *pDst        points to the block of output data
+   * @param[in]  *pScratchIn  points to a temporary buffer of size blockSize.
+   * @param[in]  *pScratchOut points to a temporary buffer of size blockSize.
+   * @param[in]  blockSize    number of input samples to process per call.
+   * @return none.
+   */
+
+  void arm_fir_sparse_q7(
+  arm_fir_sparse_instance_q7 * S,
+  q7_t * pSrc,
+  q7_t * pDst,
+  q7_t * pScratchIn,
+  q31_t * pScratchOut,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Initialization function for the Q7 sparse FIR filter.
+   * @param[in,out] *S         points to an instance of the Q7 sparse FIR structure.
+   * @param[in]     numTaps    number of nonzero coefficients in the filter.
+   * @param[in]     *pCoeffs   points to the array of filter coefficients.
+   * @param[in]     *pState    points to the state buffer.
+   * @param[in]     *pTapDelay points to the array of offset times.
+   * @param[in]     maxDelay   maximum offset time supported.
+   * @param[in]     blockSize  number of samples that will be processed per block.
+   * @return none
+   */
+
+  void arm_fir_sparse_init_q7(
+  arm_fir_sparse_instance_q7 * S,
+  uint16_t numTaps,
+  q7_t * pCoeffs,
+  q7_t * pState,
+  int32_t * pTapDelay,
+  uint16_t maxDelay,
+  uint32_t blockSize);
+
+
+  /*
+   * @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 * pCcosVal);
+
+  /*
+   * @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.
+   */
+
+  void arm_sin_cos_q31(
+  q31_t theta,
+  q31_t * pSinVal,
+  q31_t * pCosVal);
+
+
+  /**
+   * @brief  Floating-point complex conjugate.
+   * @param[in]  *pSrc points to the 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_conj_f32(
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t numSamples);
+
+  /**
+   * @brief  Q31 complex conjugate.
+   * @param[in]  *pSrc points to the 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_conj_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t numSamples);
+
+  /**
+   * @brief  Q15 complex conjugate.
+   * @param[in]  *pSrc points to the 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_conj_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t numSamples);
+
+
+
+  /**
+   * @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);
+
+  /**
+   * @brief  Q31 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_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t numSamples);
+
+  /**
+   * @brief  Q15 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_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t numSamples);
+
+
+ /**
+   * @ingroup groupController
+   */
+
+  /**
+   * @defgroup PID PID Motor Control
+   *
+   * A Proportional Integral Derivative (PID) controller is a generic feedback control
+   * loop mechanism widely used in industrial control systems.
+   * A PID controller is the most commonly used type of feedback controller.
+   *
+   * This set of functions implements (PID) controllers
+   * for Q15, Q31, and floating-point data types.  The functions operate on a single sample
+   * of data and each call to the function returns a single processed value.
+   * <code>S</code> points to an instance of the PID control data structure.  <code>in</code>
+   * is the input sample value. The functions return the output value.
+   *
+   * \par Algorithm:
+   * <pre>
+   *    y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
+   *    A0 = Kp + Ki + Kd
+   *    A1 = (-Kp ) - (2 * Kd )
+   *    A2 = Kd  </pre>
+   *
+   * \par
+   * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant
+   *
+   * \par
+   * \image html PID.gif "Proportional Integral Derivative Controller"
+   *
+   * \par
+   * The PID controller calculates an "error" value as the difference between
+   * the measured output and the reference input.
+   * The controller attempts to minimize the error by adjusting the process control inputs.
+   * The proportional value determines the reaction to the current error,
+   * the integral value determines the reaction based on the sum of recent errors,
+   * and the derivative value determines the reaction based on the rate at which the error has been changing.
+   *
+   * \par Instance Structure
+   * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure.
+   * A separate instance structure must be defined for each PID Controller.
+   * There are separate instance structure declarations for each of the 3 supported data types.
+   *
+   * \par Reset Functions
+   * There is also an associated reset function for each data type which clears the state array.
+   *
+   * \par Initialization Functions
+   * There is also an associated initialization function for each data type.
+   * The initialization function performs the following operations:
+   * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains.
+   * - Zeros out the values in the state buffer.
+   *
+   * \par
+   * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function.
+   *
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the fixed-point versions of the PID Controller functions.
+   * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+  /**
+   * @addtogroup PID
+   * @{
+   */
+
+  /**
+   * @brief  Process function for the floating-point PID Control.
+   * @param[in,out] *S is an instance of the floating-point PID Control structure
+   * @param[in] in input sample to process
+   * @return out processed output sample.
+   */
+
+
+  static __INLINE float32_t arm_pid_f32(
+  arm_pid_instance_f32 * S,
+  float32_t in)
+  {
+    float32_t out;
+
+    /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]  */
+    out = (S->A0 * in) +
+      (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);
+
+    /* Update state */
+    S->state[1] = S->state[0];
+    S->state[0] = in;
+    S->state[2] = out;
+
+    /* return to application */
+    return (out);
+
+  }
+
+  /**
+   * @brief  Process function for the Q31 PID Control.
+   * @param[in,out] *S points to an instance of the Q31 PID Control structure
+   * @param[in] in input sample to process
+   * @return out processed output sample.
+   *
+   * <b>Scaling and Overflow Behavior:</b>
+   * \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 as there are four additions.
+   * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
+   */
+
+  static __INLINE q31_t arm_pid_q31(
+  arm_pid_instance_q31 * S,
+  q31_t in)
+  {
+    q63_t acc;
+    q31_t out;
+
+    /* acc = A0 * x[n]  */
+    acc = (q63_t) S->A0 * in;
+
+    /* acc += A1 * x[n-1] */
+    acc += (q63_t) S->A1 * S->state[0];
+
+    /* acc += A2 * x[n-2]  */
+    acc += (q63_t) S->A2 * S->state[1];
+
+    /* convert output to 1.31 format to add y[n-1] */
+    out = (q31_t) (acc >> 31u);
+
+    /* out += y[n-1] */
+    out += S->state[2];
+
+    /* Update state */
+    S->state[1] = S->state[0];
+    S->state[0] = in;
+    S->state[2] = out;
+
+    /* return to application */
+    return (out);
+
+  }
+
+  /**
+   * @brief  Process function for the Q15 PID Control.
+   * @param[in,out] *S points to an instance of the Q15 PID Control structure
+   * @param[in] in input sample to process
+   * @return out processed output sample.
+   *
+   * <b>Scaling and Overflow Behavior:</b>
+   * \par
+   * The function is implemented using a 64-bit internal accumulator.
+   * Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
+   * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+   * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+   * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
+   * Lastly, the accumulator is saturated to yield a result in 1.15 format.
+   */
+
+  static __INLINE q15_t arm_pid_q15(
+  arm_pid_instance_q15 * S,
+  q15_t in)
+  {
+    q63_t acc;
+    q15_t out;
+
+#ifndef ARM_MATH_CM0_FAMILY
+    __SIMD32_TYPE *vstate;
+
+    /* Implementation of PID controller */
+
+    /* acc = A0 * x[n]  */
+    acc = (q31_t) __SMUAD(S->A0, in);
+
+    /* acc += A1 * x[n-1] + A2 * x[n-2]  */
+    vstate = __SIMD32_CONST(S->state);
+    acc = __SMLALD(S->A1, (q31_t) *vstate, acc);
+
+#else
+    /* acc = A0 * x[n]  */
+    acc = ((q31_t) S->A0) * in;
+
+    /* acc += A1 * x[n-1] + A2 * x[n-2]  */
+    acc += (q31_t) S->A1 * S->state[0];
+    acc += (q31_t) S->A2 * S->state[1];
+
+#endif
+
+    /* acc += y[n-1] */
+    acc += (q31_t) S->state[2] << 15;
+
+    /* saturate the output */
+    out = (q15_t) (__SSAT((acc >> 15), 16));
+
+    /* Update state */
+    S->state[1] = S->state[0];
+    S->state[0] = in;
+    S->state[2] = out;
+
+    /* return to application */
+    return (out);
+
+  }
+
+  /**
+   * @} end of PID group
+   */
+
+
+  /**
+   * @brief Floating-point matrix inverse.
+   * @param[in]  *src points to the instance of the input floating-point matrix structure.
+   * @param[out] *dst points to the instance of the output floating-point matrix structure.
+   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+   * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR.
+   */
+
+  arm_status arm_mat_inverse_f32(
+  const arm_matrix_instance_f32 * src,
+  arm_matrix_instance_f32 * dst);
+
+
+
+  /**
+   * @ingroup groupController
+   */
+
+
+  /**
+   * @defgroup clarke Vector Clarke Transform
+   * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector.
+   * Generally the Clarke transform uses three-phase currents <code>Ia, Ib and Ic</code> to calculate currents
+   * in the two-phase orthogonal stator axis <code>Ialpha</code> and <code>Ibeta</code>.
+   * When <code>Ialpha</code> is superposed with <code>Ia</code> as shown in the figure below
+   * \image html clarke.gif Stator current space vector and its components in (a,b).
+   * and <code>Ia + Ib + Ic = 0</code>, in this condition <code>Ialpha</code> and <code>Ibeta</code>
+   * can be calculated using only <code>Ia</code> and <code>Ib</code>.
+   *
+   * The function operates on a single sample of data and each call to the function returns the processed output.
+   * The library provides separate functions for Q31 and floating-point data types.
+   * \par Algorithm
+   * \image html clarkeFormula.gif
+   * where <code>Ia</code> and <code>Ib</code> are the instantaneous stator phases and
+   * <code>pIalpha</code> and <code>pIbeta</code> are the two coordinates of time invariant vector.
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the Q31 version of the Clarke transform.
+   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+  /**
+   * @addtogroup clarke
+   * @{
+   */
+
+  /**
+   *
+   * @brief  Floating-point Clarke transform
+   * @param[in]       Ia       input three-phase coordinate <code>a</code>
+   * @param[in]       Ib       input three-phase coordinate <code>b</code>
+   * @param[out]      *pIalpha points to output two-phase orthogonal vector axis alpha
+   * @param[out]      *pIbeta  points to output two-phase orthogonal vector axis beta
+   * @return none.
+   */
+
+  static __INLINE void arm_clarke_f32(
+  float32_t Ia,
+  float32_t Ib,
+  float32_t * pIalpha,
+  float32_t * pIbeta)
+  {
+    /* Calculate pIalpha using the equation, pIalpha = Ia */
+    *pIalpha = Ia;
+
+    /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */
+    *pIbeta =
+      ((float32_t) 0.57735026919 * Ia + (float32_t) 1.15470053838 * Ib);
+
+  }
+
+  /**
+   * @brief  Clarke transform for Q31 version
+   * @param[in]       Ia       input three-phase coordinate <code>a</code>
+   * @param[in]       Ib       input three-phase coordinate <code>b</code>
+   * @param[out]      *pIalpha points to output two-phase orthogonal vector axis alpha
+   * @param[out]      *pIbeta  points to output two-phase orthogonal vector axis beta
+   * @return none.
+   *
+   * <b>Scaling and Overflow Behavior:</b>
+   * \par
+   * The function is implemented using an internal 32-bit accumulator.
+   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+   * There is saturation on the addition, hence there is no risk of overflow.
+   */
+
+  static __INLINE void arm_clarke_q31(
+  q31_t Ia,
+  q31_t Ib,
+  q31_t * pIalpha,
+  q31_t * pIbeta)
+  {
+    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
+
+    /* Calculating pIalpha from Ia by equation pIalpha = Ia */
+    *pIalpha = Ia;
+
+    /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */
+    product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30);
+
+    /* Intermediate product is calculated by (2/sqrt(3) * Ib) */
+    product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30);
+
+    /* pIbeta is calculated by adding the intermediate products */
+    *pIbeta = __QADD(product1, product2);
+  }
+
+  /**
+   * @} end of clarke group
+   */
+
+  /**
+   * @brief  Converts the elements of the Q7 vector to Q31 vector.
+   * @param[in]  *pSrc     input pointer
+   * @param[out]  *pDst    output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_q7_to_q31(
+  q7_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+
+
+
+  /**
+   * @ingroup groupController
+   */
+
+  /**
+   * @defgroup inv_clarke Vector Inverse Clarke Transform
+   * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases.
+   *
+   * The function operates on a single sample of data and each call to the function returns the processed output.
+   * The library provides separate functions for Q31 and floating-point data types.
+   * \par Algorithm
+   * \image html clarkeInvFormula.gif
+   * where <code>pIa</code> and <code>pIb</code> are the instantaneous stator phases and
+   * <code>Ialpha</code> and <code>Ibeta</code> are the two coordinates of time invariant vector.
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the Q31 version of the Clarke transform.
+   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+  /**
+   * @addtogroup inv_clarke
+   * @{
+   */
+
+   /**
+   * @brief  Floating-point Inverse Clarke transform
+   * @param[in]       Ialpha  input two-phase orthogonal vector axis alpha
+   * @param[in]       Ibeta   input two-phase orthogonal vector axis beta
+   * @param[out]      *pIa    points to output three-phase coordinate <code>a</code>
+   * @param[out]      *pIb    points to output three-phase coordinate <code>b</code>
+   * @return none.
+   */
+
+
+  static __INLINE void arm_inv_clarke_f32(
+  float32_t Ialpha,
+  float32_t Ibeta,
+  float32_t * pIa,
+  float32_t * pIb)
+  {
+    /* Calculating pIa from Ialpha by equation pIa = Ialpha */
+    *pIa = Ialpha;
+
+    /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
+    *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
+
+  }
+
+  /**
+   * @brief  Inverse Clarke transform for Q31 version
+   * @param[in]       Ialpha  input two-phase orthogonal vector axis alpha
+   * @param[in]       Ibeta   input two-phase orthogonal vector axis beta
+   * @param[out]      *pIa    points to output three-phase coordinate <code>a</code>
+   * @param[out]      *pIb    points to output three-phase coordinate <code>b</code>
+   * @return none.
+   *
+   * <b>Scaling and Overflow Behavior:</b>
+   * \par
+   * The function is implemented using an internal 32-bit accumulator.
+   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+   * There is saturation on the subtraction, hence there is no risk of overflow.
+   */
+
+  static __INLINE void arm_inv_clarke_q31(
+  q31_t Ialpha,
+  q31_t Ibeta,
+  q31_t * pIa,
+  q31_t * pIb)
+  {
+    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
+
+    /* Calculating pIa from Ialpha by equation pIa = Ialpha */
+    *pIa = Ialpha;
+
+    /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */
+    product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31);
+
+    /* Intermediate product is calculated by (1/sqrt(3) * pIb) */
+    product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31);
+
+    /* pIb is calculated by subtracting the products */
+    *pIb = __QSUB(product2, product1);
+
+  }
+
+  /**
+   * @} end of inv_clarke group
+   */
+
+  /**
+   * @brief  Converts the elements of the Q7 vector to Q15 vector.
+   * @param[in]  *pSrc     input pointer
+   * @param[out] *pDst     output pointer
+   * @param[in]  blockSize number of samples to process
+   * @return none.
+   */
+  void arm_q7_to_q15(
+  q7_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+
+
+  /**
+   * @ingroup groupController
+   */
+
+  /**
+   * @defgroup park Vector Park Transform
+   *
+   * Forward Park transform converts the input two-coordinate vector to flux and torque components.
+   * The Park transform can be used to realize the transformation of the <code>Ialpha</code> and the <code>Ibeta</code> currents
+   * from the stationary to the moving reference frame and control the spatial relationship between
+   * the stator vector current and rotor flux vector.
+   * If we consider the d axis aligned with the rotor flux, the diagram below shows the
+   * current vector and the relationship from the two reference frames:
+   * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame"
+   *
+   * The function operates on a single sample of data and each call to the function returns the processed output.
+   * The library provides separate functions for Q31 and floating-point data types.
+   * \par Algorithm
+   * \image html parkFormula.gif
+   * where <code>Ialpha</code> and <code>Ibeta</code> are the stator vector components,
+   * <code>pId</code> and <code>pIq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
+   * cosine and sine values of theta (rotor flux position).
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the Q31 version of the Park transform.
+   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+  /**
+   * @addtogroup park
+   * @{
+   */
+
+  /**
+   * @brief Floating-point Park transform
+   * @param[in]       Ialpha input two-phase vector coordinate alpha
+   * @param[in]       Ibeta  input two-phase vector coordinate beta
+   * @param[out]      *pId   points to output	rotor reference frame d
+   * @param[out]      *pIq   points to output	rotor reference frame q
+   * @param[in]       sinVal sine value of rotation angle theta
+   * @param[in]       cosVal cosine value of rotation angle theta
+   * @return none.
+   *
+   * The function implements the forward Park transform.
+   *
+   */
+
+  static __INLINE void arm_park_f32(
+  float32_t Ialpha,
+  float32_t Ibeta,
+  float32_t * pId,
+  float32_t * pIq,
+  float32_t sinVal,
+  float32_t cosVal)
+  {
+    /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */
+    *pId = Ialpha * cosVal + Ibeta * sinVal;
+
+    /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */
+    *pIq = -Ialpha * sinVal + Ibeta * cosVal;
+
+  }
+
+  /**
+   * @brief  Park transform for Q31 version
+   * @param[in]       Ialpha input two-phase vector coordinate alpha
+   * @param[in]       Ibeta  input two-phase vector coordinate beta
+   * @param[out]      *pId   points to output rotor reference frame d
+   * @param[out]      *pIq   points to output rotor reference frame q
+   * @param[in]       sinVal sine value of rotation angle theta
+   * @param[in]       cosVal cosine value of rotation angle theta
+   * @return none.
+   *
+   * <b>Scaling and Overflow Behavior:</b>
+   * \par
+   * The function is implemented using an internal 32-bit accumulator.
+   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+   * There is saturation on the addition and subtraction, hence there is no risk of overflow.
+   */
+
+
+  static __INLINE void arm_park_q31(
+  q31_t Ialpha,
+  q31_t Ibeta,
+  q31_t * pId,
+  q31_t * pIq,
+  q31_t sinVal,
+  q31_t cosVal)
+  {
+    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
+    q31_t product3, product4;                    /* Temporary variables used to store intermediate results */
+
+    /* Intermediate product is calculated by (Ialpha * cosVal) */
+    product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31);
+
+    /* Intermediate product is calculated by (Ibeta * sinVal) */
+    product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31);
+
+
+    /* Intermediate product is calculated by (Ialpha * sinVal) */
+    product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31);
+
+    /* Intermediate product is calculated by (Ibeta * cosVal) */
+    product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31);
+
+    /* Calculate pId by adding the two intermediate products 1 and 2 */
+    *pId = __QADD(product1, product2);
+
+    /* Calculate pIq by subtracting the two intermediate products 3 from 4 */
+    *pIq = __QSUB(product4, product3);
+  }
+
+  /**
+   * @} end of park group
+   */
+
+  /**
+   * @brief  Converts the elements of the Q7 vector to floating-point vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[out]  *pDst is output pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @return none.
+   */
+  void arm_q7_to_float(
+  q7_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @ingroup groupController
+   */
+
+  /**
+   * @defgroup inv_park Vector Inverse Park transform
+   * Inverse Park transform converts the input flux and torque components to two-coordinate vector.
+   *
+   * The function operates on a single sample of data and each call to the function returns the processed output.
+   * The library provides separate functions for Q31 and floating-point data types.
+   * \par Algorithm
+   * \image html parkInvFormula.gif
+   * where <code>pIalpha</code> and <code>pIbeta</code> are the stator vector components,
+   * <code>Id</code> and <code>Iq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
+   * cosine and sine values of theta (rotor flux position).
+   * \par Fixed-Point Behavior
+   * Care must be taken when using the Q31 version of the Park transform.
+   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+   * Refer to the function specific documentation below for usage guidelines.
+   */
+
+  /**
+   * @addtogroup inv_park
+   * @{
+   */
+
+   /**
+   * @brief  Floating-point Inverse Park transform
+   * @param[in]       Id        input coordinate of rotor reference frame d
+   * @param[in]       Iq        input coordinate of rotor reference frame q
+   * @param[out]      *pIalpha  points to output two-phase orthogonal vector axis alpha
+   * @param[out]      *pIbeta   points to output two-phase orthogonal vector axis beta
+   * @param[in]       sinVal    sine value of rotation angle theta
+   * @param[in]       cosVal    cosine value of rotation angle theta
+   * @return none.
+   */
+
+  static __INLINE void arm_inv_park_f32(
+  float32_t Id,
+  float32_t Iq,
+  float32_t * pIalpha,
+  float32_t * pIbeta,
+  float32_t sinVal,
+  float32_t cosVal)
+  {
+    /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */
+    *pIalpha = Id * cosVal - Iq * sinVal;
+
+    /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */
+    *pIbeta = Id * sinVal + Iq * cosVal;
+
+  }
+
+
+  /**
+   * @brief  Inverse Park transform for	Q31 version
+   * @param[in]       Id        input coordinate of rotor reference frame d
+   * @param[in]       Iq        input coordinate of rotor reference frame q
+   * @param[out]      *pIalpha  points to output two-phase orthogonal vector axis alpha
+   * @param[out]      *pIbeta   points to output two-phase orthogonal vector axis beta
+   * @param[in]       sinVal    sine value of rotation angle theta
+   * @param[in]       cosVal    cosine value of rotation angle theta
+   * @return none.
+   *
+   * <b>Scaling and Overflow Behavior:</b>
+   * \par
+   * The function is implemented using an internal 32-bit accumulator.
+   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+   * There is saturation on the addition, hence there is no risk of overflow.
+   */
+
+
+  static __INLINE void arm_inv_park_q31(
+  q31_t Id,
+  q31_t Iq,
+  q31_t * pIalpha,
+  q31_t * pIbeta,
+  q31_t sinVal,
+  q31_t cosVal)
+  {
+    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
+    q31_t product3, product4;                    /* Temporary variables used to store intermediate results */
+
+    /* Intermediate product is calculated by (Id * cosVal) */
+    product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31);
+
+    /* Intermediate product is calculated by (Iq * sinVal) */
+    product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31);
+
+
+    /* Intermediate product is calculated by (Id * sinVal) */
+    product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31);
+
+    /* Intermediate product is calculated by (Iq * cosVal) */
+    product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31);
+
+    /* Calculate pIalpha by using the two intermediate products 1 and 2 */
+    *pIalpha = __QSUB(product1, product2);
+
+    /* Calculate pIbeta by using the two intermediate products 3 and 4 */
+    *pIbeta = __QADD(product4, product3);
+
+  }
+
+  /**
+   * @} end of Inverse park group
+   */
+
+
+  /**
+   * @brief  Converts the elements of the Q31 vector to floating-point vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[out]  *pDst is output pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @return none.
+   */
+  void arm_q31_to_float(
+  q31_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @ingroup groupInterpolation
+   */
+
+  /**
+   * @defgroup LinearInterpolate Linear Interpolation
+   *
+   * Linear interpolation is a method of curve fitting using linear polynomials.
+   * Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line
+   *
+   * \par
+   * \image html LinearInterp.gif "Linear interpolation"
+   *
+   * \par
+   * A  Linear Interpolate function calculates an output value(y), for the input(x)
+   * using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values)
+   *
+   * \par Algorithm:
+   * <pre>
+   *       y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
+   *       where x0, x1 are nearest values of input x
+   *             y0, y1 are nearest values to output y
+   * </pre>
+   *
+   * \par
+   * This set of functions implements Linear interpolation process
+   * for Q7, Q15, Q31, and floating-point data types.  The functions operate on a single
+   * sample of data and each call to the function returns a single processed value.
+   * <code>S</code> points to an instance of the Linear Interpolate function data structure.
+   * <code>x</code> is the input sample value. The functions returns the output value.
+   *
+   * \par
+   * if x is outside of the table boundary, Linear interpolation returns first value of the table
+   * if x is below input range and returns last value of table if x is above range.
+   */
+
+  /**
+   * @addtogroup LinearInterpolate
+   * @{
+   */
+
+  /**
+   * @brief  Process function for the floating-point Linear Interpolation Function.
+   * @param[in,out] *S is an instance of the floating-point Linear Interpolation structure
+   * @param[in] x input sample to process
+   * @return y processed output sample.
+   *
+   */
+
+  static __INLINE float32_t arm_linear_interp_f32(
+  arm_linear_interp_instance_f32 * S,
+  float32_t x)
+  {
+
+    float32_t y;
+    float32_t x0, x1;                            /* Nearest input values */
+    float32_t y0, y1;                            /* Nearest output values */
+    float32_t xSpacing = S->xSpacing;            /* spacing between input values */
+    int32_t i;                                   /* Index variable */
+    float32_t *pYData = S->pYData;               /* pointer to output table */
+
+    /* Calculation of index */
+    i = (int32_t) ((x - S->x1) / xSpacing);
+
+    if(i < 0)
+    {
+      /* Iniatilize output for below specified range as least output value of table */
+      y = pYData[0];
+    }
+    else if((uint32_t)i >= S->nValues)
+    {
+      /* Iniatilize output for above specified range as last output value of table */
+      y = pYData[S->nValues - 1];
+    }
+    else
+    {
+      /* Calculation of nearest input values */
+      x0 = S->x1 + i * xSpacing;
+      x1 = S->x1 + (i + 1) * xSpacing;
+
+      /* Read of nearest output values */
+      y0 = pYData[i];
+      y1 = pYData[i + 1];
+
+      /* Calculation of output */
+      y = y0 + (x - x0) * ((y1 - y0) / (x1 - x0));
+
+    }
+
+    /* returns output value */
+    return (y);
+  }
+
+   /**
+   *
+   * @brief  Process function for the Q31 Linear Interpolation Function.
+   * @param[in] *pYData  pointer to Q31 Linear Interpolation table
+   * @param[in] x input sample to process
+   * @param[in] nValues number of table values
+   * @return y processed output sample.
+   *
+   * \par
+   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
+   * This function can support maximum of table size 2^12.
+   *
+   */
+
+
+  static __INLINE q31_t arm_linear_interp_q31(
+  q31_t * pYData,
+  q31_t x,
+  uint32_t nValues)
+  {
+    q31_t y;                                     /* output */
+    q31_t y0, y1;                                /* Nearest output values */
+    q31_t fract;                                 /* fractional part */
+    int32_t index;                               /* Index to read nearest output values */
+
+    /* Input is in 12.20 format */
+    /* 12 bits for the table index */
+    /* Index value calculation */
+    index = ((x & 0xFFF00000) >> 20);
+
+    if(index >= (int32_t)(nValues - 1))
+    {
+      return (pYData[nValues - 1]);
+    }
+    else if(index < 0)
+    {
+      return (pYData[0]);
+    }
+    else
+    {
+
+      /* 20 bits for the fractional part */
+      /* shift left by 11 to keep fract in 1.31 format */
+      fract = (x & 0x000FFFFF) << 11;
+
+      /* Read two nearest output values from the index in 1.31(q31) format */
+      y0 = pYData[index];
+      y1 = pYData[index + 1u];
+
+      /* Calculation of y0 * (1-fract) and y is in 2.30 format */
+      y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32));
+
+      /* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */
+      y += ((q31_t) (((q63_t) y1 * fract) >> 32));
+
+      /* Convert y to 1.31 format */
+      return (y << 1u);
+
+    }
+
+  }
+
+  /**
+   *
+   * @brief  Process function for the Q15 Linear Interpolation Function.
+   * @param[in] *pYData  pointer to Q15 Linear Interpolation table
+   * @param[in] x input sample to process
+   * @param[in] nValues number of table values
+   * @return y processed output sample.
+   *
+   * \par
+   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
+   * This function can support maximum of table size 2^12.
+   *
+   */
+
+
+  static __INLINE q15_t arm_linear_interp_q15(
+  q15_t * pYData,
+  q31_t x,
+  uint32_t nValues)
+  {
+    q63_t y;                                     /* output */
+    q15_t y0, y1;                                /* Nearest output values */
+    q31_t fract;                                 /* fractional part */
+    int32_t index;                               /* Index to read nearest output values */
+
+    /* Input is in 12.20 format */
+    /* 12 bits for the table index */
+    /* Index value calculation */
+    index = ((x & 0xFFF00000) >> 20u);
+
+    if(index >= (int32_t)(nValues - 1))
+    {
+      return (pYData[nValues - 1]);
+    }
+    else if(index < 0)
+    {
+      return (pYData[0]);
+    }
+    else
+    {
+      /* 20 bits for the fractional part */
+      /* fract is in 12.20 format */
+      fract = (x & 0x000FFFFF);
+
+      /* Read two nearest output values from the index */
+      y0 = pYData[index];
+      y1 = pYData[index + 1u];
+
+      /* Calculation of y0 * (1-fract) and y is in 13.35 format */
+      y = ((q63_t) y0 * (0xFFFFF - fract));
+
+      /* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */
+      y += ((q63_t) y1 * (fract));
+
+      /* convert y to 1.15 format */
+      return (y >> 20);
+    }
+
+
+  }
+
+  /**
+   *
+   * @brief  Process function for the Q7 Linear Interpolation Function.
+   * @param[in] *pYData  pointer to Q7 Linear Interpolation table
+   * @param[in] x input sample to process
+   * @param[in] nValues number of table values
+   * @return y processed output sample.
+   *
+   * \par
+   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
+   * This function can support maximum of table size 2^12.
+   */
+
+
+  static __INLINE q7_t arm_linear_interp_q7(
+  q7_t * pYData,
+  q31_t x,
+  uint32_t nValues)
+  {
+    q31_t y;                                     /* output */
+    q7_t y0, y1;                                 /* Nearest output values */
+    q31_t fract;                                 /* fractional part */
+    uint32_t index;                              /* Index to read nearest output values */
+
+    /* Input is in 12.20 format */
+    /* 12 bits for the table index */
+    /* Index value calculation */
+    if (x < 0)
+    {
+      return (pYData[0]);
+    }
+    index = (x >> 20) & 0xfff;
+
+
+    if(index >= (nValues - 1))
+    {
+      return (pYData[nValues - 1]);
+    }
+    else
+    {
+
+      /* 20 bits for the fractional part */
+      /* fract is in 12.20 format */
+      fract = (x & 0x000FFFFF);
+
+      /* Read two nearest output values from the index and are in 1.7(q7) format */
+      y0 = pYData[index];
+      y1 = pYData[index + 1u];
+
+      /* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */
+      y = ((y0 * (0xFFFFF - fract)));
+
+      /* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */
+      y += (y1 * fract);
+
+      /* convert y to 1.7(q7) format */
+      return (y >> 20u);
+
+    }
+
+  }
+  /**
+   * @} end of LinearInterpolate group
+   */
+
+  /**
+   * @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);
+
+  /**
+   * @brief  Fast approximation to the trigonometric sine function for Q31 data.
+   * @param[in] x Scaled input value in radians.
+   * @return  sin(x).
+   */
+
+  q31_t arm_sin_q31(
+  q31_t x);
+
+  /**
+   * @brief  Fast approximation to the trigonometric sine function for Q15 data.
+   * @param[in] x Scaled input value in radians.
+   * @return  sin(x).
+   */
+
+  q15_t arm_sin_q15(
+  q15_t x);
+
+  /**
+   * @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);
+
+  /**
+   * @brief Fast approximation to the trigonometric cosine function for Q31 data.
+   * @param[in] x Scaled input value in radians.
+   * @return  cos(x).
+   */
+
+  q31_t arm_cos_q31(
+  q31_t x);
+
+  /**
+   * @brief  Fast approximation to the trigonometric cosine function for Q15 data.
+   * @param[in] x Scaled input value in radians.
+   * @return  cos(x).
+   */
+
+  q15_t arm_cos_q15(
+  q15_t x);
+
+
+  /**
+   * @ingroup groupFastMath
+   */
+
+
+  /**
+   * @defgroup SQRT Square Root
+   *
+   * Computes the square root of a number.
+   * There are separate functions for Q15, Q31, and floating-point data types.
+   * The square root function is computed using the Newton-Raphson algorithm.
+   * This is an iterative algorithm of the form:
+   * <pre>
+   *      x1 = x0 - f(x0)/f'(x0)
+   * </pre>
+   * where <code>x1</code> is the current estimate,
+   * <code>x0</code> is the previous estimate, and
+   * <code>f'(x0)</code> is the derivative of <code>f()</code> evaluated at <code>x0</code>.
+   * For the square root function, the algorithm reduces to:
+   * <pre>
+   *     x0 = in/2                         [initial guess]
+   *     x1 = 1/2 * ( x0 + in / x0)        [each iteration]
+   * </pre>
+   */
+
+
+  /**
+   * @addtogroup SQRT
+   * @{
+   */
+
+  /**
+   * @brief  Floating-point square root function.
+   * @param[in]  in     input value.
+   * @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.
+   */
+
+  static __INLINE arm_status arm_sqrt_f32(
+  float32_t in,
+  float32_t * pOut)
+  {
+    if(in > 0)
+    {
+
+//      #if __FPU_USED
+#if (__FPU_USED == 1) && defined ( __CC_ARM   )
+      *pOut = __sqrtf(in);
+#else
+      *pOut = sqrtf(in);
+#endif
+
+      return (ARM_MATH_SUCCESS);
+    }
+    else
+    {
+      *pOut = 0.0f;
+      return (ARM_MATH_ARGUMENT_ERROR);
+    }
+
+  }
+
+
+  /**
+   * @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);
+
+  /**
+   * @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);
+
+  /**
+   * @} end of SQRT group
+   */
+
+
+
+
+
+
+  /**
+   * @brief floating-point Circular write function.
+   */
+
+  static __INLINE void arm_circularWrite_f32(
+  int32_t * circBuffer,
+  int32_t L,
+  uint16_t * writeOffset,
+  int32_t bufferInc,
+  const int32_t * src,
+  int32_t srcInc,
+  uint32_t blockSize)
+  {
+    uint32_t i = 0u;
+    int32_t wOffset;
+
+    /* Copy the value of Index pointer that points
+     * to the current location where the input samples to be copied */
+    wOffset = *writeOffset;
+
+    /* Loop over the blockSize */
+    i = blockSize;
+
+    while(i > 0u)
+    {
+      /* copy the input sample to the circular buffer */
+      circBuffer[wOffset] = *src;
+
+      /* Update the input pointer */
+      src += srcInc;
+
+      /* Circularly update wOffset.  Watch out for positive and negative value */
+      wOffset += bufferInc;
+      if(wOffset >= L)
+        wOffset -= L;
+
+      /* Decrement the loop counter */
+      i--;
+    }
+
+    /* Update the index pointer */
+    *writeOffset = wOffset;
+  }
+
+
+
+  /**
+   * @brief floating-point Circular Read function.
+   */
+  static __INLINE void arm_circularRead_f32(
+  int32_t * circBuffer,
+  int32_t L,
+  int32_t * readOffset,
+  int32_t bufferInc,
+  int32_t * dst,
+  int32_t * dst_base,
+  int32_t dst_length,
+  int32_t dstInc,
+  uint32_t blockSize)
+  {
+    uint32_t i = 0u;
+    int32_t rOffset, dst_end;
+
+    /* Copy the value of Index pointer that points
+     * to the current location from where the input samples to be read */
+    rOffset = *readOffset;
+    dst_end = (int32_t) (dst_base + dst_length);
+
+    /* Loop over the blockSize */
+    i = blockSize;
+
+    while(i > 0u)
+    {
+      /* copy the sample from the circular buffer to the destination buffer */
+      *dst = circBuffer[rOffset];
+
+      /* Update the input pointer */
+      dst += dstInc;
+
+      if(dst == (int32_t *) dst_end)
+      {
+        dst = dst_base;
+      }
+
+      /* Circularly update rOffset.  Watch out for positive and negative value  */
+      rOffset += bufferInc;
+
+      if(rOffset >= L)
+      {
+        rOffset -= L;
+      }
+
+      /* Decrement the loop counter */
+      i--;
+    }
+
+    /* Update the index pointer */
+    *readOffset = rOffset;
+  }
+
+  /**
+   * @brief Q15 Circular write function.
+   */
+
+  static __INLINE void arm_circularWrite_q15(
+  q15_t * circBuffer,
+  int32_t L,
+  uint16_t * writeOffset,
+  int32_t bufferInc,
+  const q15_t * src,
+  int32_t srcInc,
+  uint32_t blockSize)
+  {
+    uint32_t i = 0u;
+    int32_t wOffset;
+
+    /* Copy the value of Index pointer that points
+     * to the current location where the input samples to be copied */
+    wOffset = *writeOffset;
+
+    /* Loop over the blockSize */
+    i = blockSize;
+
+    while(i > 0u)
+    {
+      /* copy the input sample to the circular buffer */
+      circBuffer[wOffset] = *src;
+
+      /* Update the input pointer */
+      src += srcInc;
+
+      /* Circularly update wOffset.  Watch out for positive and negative value */
+      wOffset += bufferInc;
+      if(wOffset >= L)
+        wOffset -= L;
+
+      /* Decrement the loop counter */
+      i--;
+    }
+
+    /* Update the index pointer */
+    *writeOffset = wOffset;
+  }
+
+
+
+  /**
+   * @brief Q15 Circular Read function.
+   */
+  static __INLINE void arm_circularRead_q15(
+  q15_t * circBuffer,
+  int32_t L,
+  int32_t * readOffset,
+  int32_t bufferInc,
+  q15_t * dst,
+  q15_t * dst_base,
+  int32_t dst_length,
+  int32_t dstInc,
+  uint32_t blockSize)
+  {
+    uint32_t i = 0;
+    int32_t rOffset, dst_end;
+
+    /* Copy the value of Index pointer that points
+     * to the current location from where the input samples to be read */
+    rOffset = *readOffset;
+
+    dst_end = (int32_t) (dst_base + dst_length);
+
+    /* Loop over the blockSize */
+    i = blockSize;
+
+    while(i > 0u)
+    {
+      /* copy the sample from the circular buffer to the destination buffer */
+      *dst = circBuffer[rOffset];
+
+      /* Update the input pointer */
+      dst += dstInc;
+
+      if(dst == (q15_t *) dst_end)
+      {
+        dst = dst_base;
+      }
+
+      /* Circularly update wOffset.  Watch out for positive and negative value */
+      rOffset += bufferInc;
+
+      if(rOffset >= L)
+      {
+        rOffset -= L;
+      }
+
+      /* Decrement the loop counter */
+      i--;
+    }
+
+    /* Update the index pointer */
+    *readOffset = rOffset;
+  }
+
+
+  /**
+   * @brief Q7 Circular write function.
+   */
+
+  static __INLINE void arm_circularWrite_q7(
+  q7_t * circBuffer,
+  int32_t L,
+  uint16_t * writeOffset,
+  int32_t bufferInc,
+  const q7_t * src,
+  int32_t srcInc,
+  uint32_t blockSize)
+  {
+    uint32_t i = 0u;
+    int32_t wOffset;
+
+    /* Copy the value of Index pointer that points
+     * to the current location where the input samples to be copied */
+    wOffset = *writeOffset;
+
+    /* Loop over the blockSize */
+    i = blockSize;
+
+    while(i > 0u)
+    {
+      /* copy the input sample to the circular buffer */
+      circBuffer[wOffset] = *src;
+
+      /* Update the input pointer */
+      src += srcInc;
+
+      /* Circularly update wOffset.  Watch out for positive and negative value */
+      wOffset += bufferInc;
+      if(wOffset >= L)
+        wOffset -= L;
+
+      /* Decrement the loop counter */
+      i--;
+    }
+
+    /* Update the index pointer */
+    *writeOffset = wOffset;
+  }
+
+
+
+  /**
+   * @brief Q7 Circular Read function.
+   */
+  static __INLINE void arm_circularRead_q7(
+  q7_t * circBuffer,
+  int32_t L,
+  int32_t * readOffset,
+  int32_t bufferInc,
+  q7_t * dst,
+  q7_t * dst_base,
+  int32_t dst_length,
+  int32_t dstInc,
+  uint32_t blockSize)
+  {
+    uint32_t i = 0;
+    int32_t rOffset, dst_end;
+
+    /* Copy the value of Index pointer that points
+     * to the current location from where the input samples to be read */
+    rOffset = *readOffset;
+
+    dst_end = (int32_t) (dst_base + dst_length);
+
+    /* Loop over the blockSize */
+    i = blockSize;
+
+    while(i > 0u)
+    {
+      /* copy the sample from the circular buffer to the destination buffer */
+      *dst = circBuffer[rOffset];
+
+      /* Update the input pointer */
+      dst += dstInc;
+
+      if(dst == (q7_t *) dst_end)
+      {
+        dst = dst_base;
+      }
+
+      /* Circularly update rOffset.  Watch out for positive and negative value */
+      rOffset += bufferInc;
+
+      if(rOffset >= L)
+      {
+        rOffset -= L;
+      }
+
+      /* Decrement the loop counter */
+      i--;
+    }
+
+    /* Update the index pointer */
+    *readOffset = rOffset;
+  }
+
+
+  /**
+   * @brief  Sum of the squares of the elements of a Q31 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_power_q31(
+  q31_t * pSrc,
+  uint32_t blockSize,
+  q63_t * pResult);
+
+  /**
+   * @brief  Sum of the squares of the elements of a floating-point vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_power_f32(
+  float32_t * pSrc,
+  uint32_t blockSize,
+  float32_t * pResult);
+
+  /**
+   * @brief  Sum of the squares of the elements of a Q15 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_power_q15(
+  q15_t * pSrc,
+  uint32_t blockSize,
+  q63_t * pResult);
+
+  /**
+   * @brief  Sum of the squares of the elements of a Q7 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_power_q7(
+  q7_t * pSrc,
+  uint32_t blockSize,
+  q31_t * pResult);
+
+  /**
+   * @brief  Mean value of a Q7 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_mean_q7(
+  q7_t * pSrc,
+  uint32_t blockSize,
+  q7_t * pResult);
+
+  /**
+   * @brief  Mean value of a Q15 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+  void arm_mean_q15(
+  q15_t * pSrc,
+  uint32_t blockSize,
+  q15_t * pResult);
+
+  /**
+   * @brief  Mean value of a Q31 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+  void arm_mean_q31(
+  q31_t * pSrc,
+  uint32_t blockSize,
+  q31_t * pResult);
+
+  /**
+   * @brief  Mean value of a floating-point vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+  void arm_mean_f32(
+  float32_t * pSrc,
+  uint32_t blockSize,
+  float32_t * pResult);
+
+  /**
+   * @brief  Variance of the elements of a floating-point vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_var_f32(
+  float32_t * pSrc,
+  uint32_t blockSize,
+  float32_t * pResult);
+
+  /**
+   * @brief  Variance of the elements of a Q31 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_var_q31(
+  q31_t * pSrc,
+  uint32_t blockSize,
+  q63_t * pResult);
+
+  /**
+   * @brief  Variance of the elements of a Q15 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_var_q15(
+  q15_t * pSrc,
+  uint32_t blockSize,
+  q31_t * pResult);
+
+  /**
+   * @brief  Root Mean Square of the elements of a floating-point vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_rms_f32(
+  float32_t * pSrc,
+  uint32_t blockSize,
+  float32_t * pResult);
+
+  /**
+   * @brief  Root Mean Square of the elements of a Q31 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_rms_q31(
+  q31_t * pSrc,
+  uint32_t blockSize,
+  q31_t * pResult);
+
+  /**
+   * @brief  Root Mean Square of the elements of a Q15 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_rms_q15(
+  q15_t * pSrc,
+  uint32_t blockSize,
+  q15_t * pResult);
+
+  /**
+   * @brief  Standard deviation of the elements of a floating-point vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_std_f32(
+  float32_t * pSrc,
+  uint32_t blockSize,
+  float32_t * pResult);
+
+  /**
+   * @brief  Standard deviation of the elements of a Q31 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_std_q31(
+  q31_t * pSrc,
+  uint32_t blockSize,
+  q31_t * pResult);
+
+  /**
+   * @brief  Standard deviation of the elements of a Q15 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output value.
+   * @return none.
+   */
+
+  void arm_std_q15(
+  q15_t * pSrc,
+  uint32_t blockSize,
+  q15_t * pResult);
+
+  /**
+   * @brief  Floating-point complex magnitude
+   * @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_f32(
+  float32_t * pSrc,
+  float32_t * pDst,
+  uint32_t numSamples);
+
+  /**
+   * @brief  Q31 complex magnitude
+   * @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_q31(
+  q31_t * pSrc,
+  q31_t * pDst,
+  uint32_t numSamples);
+
+  /**
+   * @brief  Q15 complex magnitude
+   * @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_q15(
+  q15_t * pSrc,
+  q15_t * pDst,
+  uint32_t numSamples);
+
+  /**
+   * @brief  Q15 complex dot product
+   * @param[in]  *pSrcA points to the first input vector
+   * @param[in]  *pSrcB points to the second input vector
+   * @param[in]  numSamples number of complex samples in each vector
+   * @param[out]  *realResult real part of the result returned here
+   * @param[out]  *imagResult imaginary part of the result returned here
+   * @return none.
+   */
+
+  void arm_cmplx_dot_prod_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  uint32_t numSamples,
+  q31_t * realResult,
+  q31_t * imagResult);
+
+  /**
+   * @brief  Q31 complex dot product
+   * @param[in]  *pSrcA points to the first input vector
+   * @param[in]  *pSrcB points to the second input vector
+   * @param[in]  numSamples number of complex samples in each vector
+   * @param[out]  *realResult real part of the result returned here
+   * @param[out]  *imagResult imaginary part of the result returned here
+   * @return none.
+   */
+
+  void arm_cmplx_dot_prod_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  uint32_t numSamples,
+  q63_t * realResult,
+  q63_t * imagResult);
+
+  /**
+   * @brief  Floating-point complex dot product
+   * @param[in]  *pSrcA points to the first input vector
+   * @param[in]  *pSrcB points to the second input vector
+   * @param[in]  numSamples number of complex samples in each vector
+   * @param[out]  *realResult real part of the result returned here
+   * @param[out]  *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);
+
+  /**
+   * @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.
+   */
+
+  void arm_cmplx_mult_real_q15(
+  q15_t * pSrcCmplx,
+  q15_t * pSrcReal,
+  q15_t * pCmplxDst,
+  uint32_t numSamples);
+
+  /**
+   * @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.
+   */
+
+  void arm_cmplx_mult_real_q31(
+  q31_t * pSrcCmplx,
+  q31_t * pSrcReal,
+  q31_t * pCmplxDst,
+  uint32_t numSamples);
+
+  /**
+   * @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);
+
+  /**
+   * @brief  Minimum value of a Q7 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *result is output pointer
+   * @param[in]  index is the array index of the minimum value in the input buffer.
+   * @return none.
+   */
+
+  void arm_min_q7(
+  q7_t * pSrc,
+  uint32_t blockSize,
+  q7_t * result,
+  uint32_t * index);
+
+  /**
+   * @brief  Minimum value of a Q15 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output pointer
+   * @param[in]  *pIndex is the array index of the minimum value in the input buffer.
+   * @return none.
+   */
+
+  void arm_min_q15(
+  q15_t * pSrc,
+  uint32_t blockSize,
+  q15_t * pResult,
+  uint32_t * pIndex);
+
+  /**
+   * @brief  Minimum value of a Q31 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output pointer
+   * @param[out]  *pIndex is the array index of the minimum value in the input buffer.
+   * @return none.
+   */
+  void arm_min_q31(
+  q31_t * pSrc,
+  uint32_t blockSize,
+  q31_t * pResult,
+  uint32_t * pIndex);
+
+  /**
+   * @brief  Minimum value of a floating-point vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @param[out]  *pResult is output pointer
+   * @param[out]  *pIndex is the array index of the minimum value in the input buffer.
+   * @return none.
+   */
+
+  void arm_min_f32(
+  float32_t * pSrc,
+  uint32_t blockSize,
+  float32_t * pResult,
+  uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of a Q7 vector.
+ * @param[in]       *pSrc points to the input buffer
+ * @param[in]       blockSize length of the input vector
+ * @param[out]      *pResult maximum value returned here
+ * @param[out]      *pIndex index of maximum value returned here
+ * @return none.
+ */
+
+  void arm_max_q7(
+  q7_t * pSrc,
+  uint32_t blockSize,
+  q7_t * pResult,
+  uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of a Q15 vector.
+ * @param[in]       *pSrc points to the input buffer
+ * @param[in]       blockSize length of the input vector
+ * @param[out]      *pResult maximum value returned here
+ * @param[out]      *pIndex index of maximum value returned here
+ * @return none.
+ */
+
+  void arm_max_q15(
+  q15_t * pSrc,
+  uint32_t blockSize,
+  q15_t * pResult,
+  uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of a Q31 vector.
+ * @param[in]       *pSrc points to the input buffer
+ * @param[in]       blockSize length of the input vector
+ * @param[out]      *pResult maximum value returned here
+ * @param[out]      *pIndex index of maximum value returned here
+ * @return none.
+ */
+
+  void arm_max_q31(
+  q31_t * pSrc,
+  uint32_t blockSize,
+  q31_t * pResult,
+  uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of a floating-point vector.
+ * @param[in]       *pSrc points to the input buffer
+ * @param[in]       blockSize length of the input vector
+ * @param[out]      *pResult maximum value returned here
+ * @param[out]      *pIndex index of maximum value returned here
+ * @return none.
+ */
+
+  void arm_max_f32(
+  float32_t * pSrc,
+  uint32_t blockSize,
+  float32_t * pResult,
+  uint32_t * pIndex);
+
+  /**
+   * @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.
+   */
+
+  void arm_cmplx_mult_cmplx_q15(
+  q15_t * pSrcA,
+  q15_t * pSrcB,
+  q15_t * pDst,
+  uint32_t numSamples);
+
+  /**
+   * @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.
+   */
+
+  void arm_cmplx_mult_cmplx_q31(
+  q31_t * pSrcA,
+  q31_t * pSrcB,
+  q31_t * pDst,
+  uint32_t numSamples);
+
+  /**
+   * @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);
+
+  /**
+   * @brief Converts the elements of the floating-point vector to Q31 vector.
+   * @param[in]       *pSrc points to the floating-point input vector
+   * @param[out]      *pDst points to the Q31 output vector
+   * @param[in]       blockSize length of the input vector
+   * @return none.
+   */
+  void arm_float_to_q31(
+  float32_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Converts the elements of the floating-point vector to Q15 vector.
+   * @param[in]       *pSrc points to the floating-point input vector
+   * @param[out]      *pDst points to the Q15 output vector
+   * @param[in]       blockSize length of the input vector
+   * @return          none
+   */
+  void arm_float_to_q15(
+  float32_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief Converts the elements of the floating-point vector to Q7 vector.
+   * @param[in]       *pSrc points to the floating-point input vector
+   * @param[out]      *pDst points to the Q7 output vector
+   * @param[in]       blockSize length of the input vector
+   * @return          none
+   */
+  void arm_float_to_q7(
+  float32_t * pSrc,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q31 vector to Q15 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[out]  *pDst is output pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @return none.
+   */
+  void arm_q31_to_q15(
+  q31_t * pSrc,
+  q15_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Converts the elements of the Q31 vector to Q7 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[out]  *pDst is output pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @return none.
+   */
+  void arm_q31_to_q7(
+  q31_t * pSrc,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+  /**
+   * @brief  Converts the elements of the Q15 vector to floating-point vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[out]  *pDst is output pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @return none.
+   */
+  void arm_q15_to_float(
+  q15_t * pSrc,
+  float32_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q15 vector to Q31 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[out]  *pDst is output pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @return none.
+   */
+  void arm_q15_to_q31(
+  q15_t * pSrc,
+  q31_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @brief  Converts the elements of the Q15 vector to Q7 vector.
+   * @param[in]  *pSrc is input pointer
+   * @param[out]  *pDst is output pointer
+   * @param[in]  blockSize is the number of samples to process
+   * @return none.
+   */
+  void arm_q15_to_q7(
+  q15_t * pSrc,
+  q7_t * pDst,
+  uint32_t blockSize);
+
+
+  /**
+   * @ingroup groupInterpolation
+   */
+
+  /**
+   * @defgroup BilinearInterpolate Bilinear Interpolation
+   *
+   * Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid.
+   * The underlying function <code>f(x, y)</code> is sampled on a regular grid and the interpolation process
+   * determines values between the grid points.
+   * Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension.
+   * Bilinear interpolation is often used in image processing to rescale images.
+   * The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types.
+   *
+   * <b>Algorithm</b>
+   * \par
+   * The instance structure used by the bilinear interpolation functions describes a two dimensional data table.
+   * For floating-point, the instance structure is defined as:
+   * <pre>
+   *   typedef struct
+   *   {
+   *     uint16_t numRows;
+   *     uint16_t numCols;
+   *     float32_t *pData;
+   * } arm_bilinear_interp_instance_f32;
+   * </pre>
+   *
+   * \par
+   * where <code>numRows</code> specifies the number of rows in the table;
+   * <code>numCols</code> specifies the number of columns in the table;
+   * and <code>pData</code> points to an array of size <code>numRows*numCols</code> values.
+   * The data table <code>pTable</code> is organized in row order and the supplied data values fall on integer indexes.
+   * That is, table element (x,y) is located at <code>pTable[x + y*numCols]</code> where x and y are integers.
+   *
+   * \par
+   * Let <code>(x, y)</code> specify the desired interpolation point.  Then define:
+   * <pre>
+   *     XF = floor(x)
+   *     YF = floor(y)
+   * </pre>
+   * \par
+   * The interpolated output point is computed as:
+   * <pre>
+   *  f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))
+   *           + f(XF+1, YF) * (x-XF)*(1-(y-YF))
+   *           + f(XF, YF+1) * (1-(x-XF))*(y-YF)
+   *           + f(XF+1, YF+1) * (x-XF)*(y-YF)
+   * </pre>
+   * Note that the coordinates (x, y) contain integer and fractional components.
+   * The integer components specify which portion of the table to use while the
+   * fractional components control the interpolation processor.
+   *
+   * \par
+   * if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output.
+   */
+
+  /**
+   * @addtogroup BilinearInterpolate
+   * @{
+   */
+
+  /**
+  *
+  * @brief  Floating-point bilinear interpolation.
+  * @param[in,out] *S points to an instance of the interpolation structure.
+  * @param[in] X interpolation coordinate.
+  * @param[in] Y interpolation coordinate.
+  * @return out interpolated value.
+  */
+
+
+  static __INLINE float32_t arm_bilinear_interp_f32(
+  const arm_bilinear_interp_instance_f32 * S,
+  float32_t X,
+  float32_t Y)
+  {
+    float32_t out;
+    float32_t f00, f01, f10, f11;
+    float32_t *pData = S->pData;
+    int32_t xIndex, yIndex, index;
+    float32_t xdiff, ydiff;
+    float32_t b1, b2, b3, b4;
+
+    xIndex = (int32_t) X;
+    yIndex = (int32_t) Y;
+
+    /* Care taken for table outside boundary */
+    /* Returns zero output when values are outside table boundary */
+    if(xIndex < 0 || xIndex > (S->numRows - 1) || yIndex < 0
+       || yIndex > (S->numCols - 1))
+    {
+      return (0);
+    }
+
+    /* Calculation of index for two nearest points in X-direction */
+    index = (xIndex - 1) + (yIndex - 1) * S->numCols;
+
+
+    /* Read two nearest points in X-direction */
+    f00 = pData[index];
+    f01 = pData[index + 1];
+
+    /* Calculation of index for two nearest points in Y-direction */
+    index = (xIndex - 1) + (yIndex) * S->numCols;
+
+
+    /* Read two nearest points in Y-direction */
+    f10 = pData[index];
+    f11 = pData[index + 1];
+
+    /* Calculation of intermediate values */
+    b1 = f00;
+    b2 = f01 - f00;
+    b3 = f10 - f00;
+    b4 = f00 - f01 - f10 + f11;
+
+    /* Calculation of fractional part in X */
+    xdiff = X - xIndex;
+
+    /* Calculation of fractional part in Y */
+    ydiff = Y - yIndex;
+
+    /* Calculation of bi-linear interpolated output */
+    out = b1 + b2 * xdiff + b3 * ydiff + b4 * xdiff * ydiff;
+
+    /* return to application */
+    return (out);
+
+  }
+
+  /**
+  *
+  * @brief  Q31 bilinear interpolation.
+  * @param[in,out] *S points to an instance of the interpolation structure.
+  * @param[in] X interpolation coordinate in 12.20 format.
+  * @param[in] Y interpolation coordinate in 12.20 format.
+  * @return out interpolated value.
+  */
+
+  static __INLINE q31_t arm_bilinear_interp_q31(
+  arm_bilinear_interp_instance_q31 * S,
+  q31_t X,
+  q31_t Y)
+  {
+    q31_t out;                                   /* Temporary output */
+    q31_t acc = 0;                               /* output */
+    q31_t xfract, yfract;                        /* X, Y fractional parts */
+    q31_t x1, x2, y1, y2;                        /* Nearest output values */
+    int32_t rI, cI;                              /* Row and column indices */
+    q31_t *pYData = S->pData;                    /* pointer to output table values */
+    uint32_t nCols = S->numCols;                 /* num of rows */
+
+
+    /* Input is in 12.20 format */
+    /* 12 bits for the table index */
+    /* Index value calculation */
+    rI = ((X & 0xFFF00000) >> 20u);
+
+    /* Input is in 12.20 format */
+    /* 12 bits for the table index */
+    /* Index value calculation */
+    cI = ((Y & 0xFFF00000) >> 20u);
+
+    /* Care taken for table outside boundary */
+    /* Returns zero output when values are outside table boundary */
+    if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
+    {
+      return (0);
+    }
+
+    /* 20 bits for the fractional part */
+    /* shift left xfract by 11 to keep 1.31 format */
+    xfract = (X & 0x000FFFFF) << 11u;
+
+    /* Read two nearest output values from the index */
+    x1 = pYData[(rI) + nCols * (cI)];
+    x2 = pYData[(rI) + nCols * (cI) + 1u];
+
+    /* 20 bits for the fractional part */
+    /* shift left yfract by 11 to keep 1.31 format */
+    yfract = (Y & 0x000FFFFF) << 11u;
+
+    /* Read two nearest output values from the index */
+    y1 = pYData[(rI) + nCols * (cI + 1)];
+    y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
+
+    /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 3.29(q29) format */
+    out = ((q31_t) (((q63_t) x1 * (0x7FFFFFFF - xfract)) >> 32));
+    acc = ((q31_t) (((q63_t) out * (0x7FFFFFFF - yfract)) >> 32));
+
+    /* x2 * (xfract) * (1-yfract)  in 3.29(q29) and adding to acc */
+    out = ((q31_t) ((q63_t) x2 * (0x7FFFFFFF - yfract) >> 32));
+    acc += ((q31_t) ((q63_t) out * (xfract) >> 32));
+
+    /* y1 * (1 - xfract) * (yfract)  in 3.29(q29) and adding to acc */
+    out = ((q31_t) ((q63_t) y1 * (0x7FFFFFFF - xfract) >> 32));
+    acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
+
+    /* y2 * (xfract) * (yfract)  in 3.29(q29) and adding to acc */
+    out = ((q31_t) ((q63_t) y2 * (xfract) >> 32));
+    acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
+
+    /* Convert acc to 1.31(q31) format */
+    return (acc << 2u);
+
+  }
+
+  /**
+  * @brief  Q15 bilinear interpolation.
+  * @param[in,out] *S points to an instance of the interpolation structure.
+  * @param[in] X interpolation coordinate in 12.20 format.
+  * @param[in] Y interpolation coordinate in 12.20 format.
+  * @return out interpolated value.
+  */
+
+  static __INLINE q15_t arm_bilinear_interp_q15(
+  arm_bilinear_interp_instance_q15 * S,
+  q31_t X,
+  q31_t Y)
+  {
+    q63_t acc = 0;                               /* output */
+    q31_t out;                                   /* Temporary output */
+    q15_t x1, x2, y1, y2;                        /* Nearest output values */
+    q31_t xfract, yfract;                        /* X, Y fractional parts */
+    int32_t rI, cI;                              /* Row and column indices */
+    q15_t *pYData = S->pData;                    /* pointer to output table values */
+    uint32_t nCols = S->numCols;                 /* num of rows */
+
+    /* Input is in 12.20 format */
+    /* 12 bits for the table index */
+    /* Index value calculation */
+    rI = ((X & 0xFFF00000) >> 20);
+
+    /* Input is in 12.20 format */
+    /* 12 bits for the table index */
+    /* Index value calculation */
+    cI = ((Y & 0xFFF00000) >> 20);
+
+    /* Care taken for table outside boundary */
+    /* Returns zero output when values are outside table boundary */
+    if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
+    {
+      return (0);
+    }
+
+    /* 20 bits for the fractional part */
+    /* xfract should be in 12.20 format */
+    xfract = (X & 0x000FFFFF);
+
+    /* Read two nearest output values from the index */
+    x1 = pYData[(rI) + nCols * (cI)];
+    x2 = pYData[(rI) + nCols * (cI) + 1u];
+
+
+    /* 20 bits for the fractional part */
+    /* yfract should be in 12.20 format */
+    yfract = (Y & 0x000FFFFF);
+
+    /* Read two nearest output values from the index */
+    y1 = pYData[(rI) + nCols * (cI + 1)];
+    y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
+
+    /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 13.51 format */
+
+    /* x1 is in 1.15(q15), xfract in 12.20 format and out is in 13.35 format */
+    /* convert 13.35 to 13.31 by right shifting  and out is in 1.31 */
+    out = (q31_t) (((q63_t) x1 * (0xFFFFF - xfract)) >> 4u);
+    acc = ((q63_t) out * (0xFFFFF - yfract));
+
+    /* x2 * (xfract) * (1-yfract)  in 1.51 and adding to acc */
+    out = (q31_t) (((q63_t) x2 * (0xFFFFF - yfract)) >> 4u);
+    acc += ((q63_t) out * (xfract));
+
+    /* y1 * (1 - xfract) * (yfract)  in 1.51 and adding to acc */
+    out = (q31_t) (((q63_t) y1 * (0xFFFFF - xfract)) >> 4u);
+    acc += ((q63_t) out * (yfract));
+
+    /* y2 * (xfract) * (yfract)  in 1.51 and adding to acc */
+    out = (q31_t) (((q63_t) y2 * (xfract)) >> 4u);
+    acc += ((q63_t) out * (yfract));
+
+    /* acc is in 13.51 format and down shift acc by 36 times */
+    /* Convert out to 1.15 format */
+    return (acc >> 36);
+
+  }
+
+  /**
+  * @brief  Q7 bilinear interpolation.
+  * @param[in,out] *S points to an instance of the interpolation structure.
+  * @param[in] X interpolation coordinate in 12.20 format.
+  * @param[in] Y interpolation coordinate in 12.20 format.
+  * @return out interpolated value.
+  */
+
+  static __INLINE q7_t arm_bilinear_interp_q7(
+  arm_bilinear_interp_instance_q7 * S,
+  q31_t X,
+  q31_t Y)
+  {
+    q63_t acc = 0;                               /* output */
+    q31_t out;                                   /* Temporary output */
+    q31_t xfract, yfract;                        /* X, Y fractional parts */
+    q7_t x1, x2, y1, y2;                         /* Nearest output values */
+    int32_t rI, cI;                              /* Row and column indices */
+    q7_t *pYData = S->pData;                     /* pointer to output table values */
+    uint32_t nCols = S->numCols;                 /* num of rows */
+
+    /* Input is in 12.20 format */
+    /* 12 bits for the table index */
+    /* Index value calculation */
+    rI = ((X & 0xFFF00000) >> 20);
+
+    /* Input is in 12.20 format */
+    /* 12 bits for the table index */
+    /* Index value calculation */
+    cI = ((Y & 0xFFF00000) >> 20);
+
+    /* Care taken for table outside boundary */
+    /* Returns zero output when values are outside table boundary */
+    if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
+    {
+      return (0);
+    }
+
+    /* 20 bits for the fractional part */
+    /* xfract should be in 12.20 format */
+    xfract = (X & 0x000FFFFF);
+
+    /* Read two nearest output values from the index */
+    x1 = pYData[(rI) + nCols * (cI)];
+    x2 = pYData[(rI) + nCols * (cI) + 1u];
+
+
+    /* 20 bits for the fractional part */
+    /* yfract should be in 12.20 format */
+    yfract = (Y & 0x000FFFFF);
+
+    /* Read two nearest output values from the index */
+    y1 = pYData[(rI) + nCols * (cI + 1)];
+    y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
+
+    /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 16.47 format */
+    out = ((x1 * (0xFFFFF - xfract)));
+    acc = (((q63_t) out * (0xFFFFF - yfract)));
+
+    /* x2 * (xfract) * (1-yfract)  in 2.22 and adding to acc */
+    out = ((x2 * (0xFFFFF - yfract)));
+    acc += (((q63_t) out * (xfract)));
+
+    /* y1 * (1 - xfract) * (yfract)  in 2.22 and adding to acc */
+    out = ((y1 * (0xFFFFF - xfract)));
+    acc += (((q63_t) out * (yfract)));
+
+    /* y2 * (xfract) * (yfract)  in 2.22 and adding to acc */
+    out = ((y2 * (yfract)));
+    acc += (((q63_t) out * (xfract)));
+
+    /* acc in 16.47 format and down shift by 40 to convert to 1.7 format */
+    return (acc >> 40);
+
+  }
+
+  /**
+   * @} end of BilinearInterpolate group
+   */
+
+
+#if   defined ( __CC_ARM ) //Keil
+//SMMLAR
+  #define multAcc_32x32_keep32_R(a, x, y) \
+  a = (q31_t) (((((q63_t) a) << 32) + ((q63_t) x * y) + 0x80000000LL ) >> 32)
+
+//SMMLSR
+  #define multSub_32x32_keep32_R(a, x, y) \
+  a = (q31_t) (((((q63_t) a) << 32) - ((q63_t) x * y) + 0x80000000LL ) >> 32)
+
+//SMMULR
+  #define mult_32x32_keep32_R(a, x, y) \
+  a = (q31_t) (((q63_t) x * y + 0x80000000LL ) >> 32)
+
+//Enter low optimization region - place directly above function definition
+  #define LOW_OPTIMIZATION_ENTER \
+     _Pragma ("push")         \
+     _Pragma ("O1")
+
+//Exit low optimization region - place directly after end of function definition
+  #define LOW_OPTIMIZATION_EXIT \
+     _Pragma ("pop")
+
+//Enter low optimization region - place directly above function definition
+  #define IAR_ONLY_LOW_OPTIMIZATION_ENTER
+
+//Exit low optimization region - place directly after end of function definition
+  #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
+
+#elif defined(__ICCARM__) //IAR
+ //SMMLA
+  #define multAcc_32x32_keep32_R(a, x, y) \
+  a += (q31_t) (((q63_t) x * y) >> 32)
+
+ //SMMLS
+  #define multSub_32x32_keep32_R(a, x, y) \
+  a -= (q31_t) (((q63_t) x * y) >> 32)
+
+//SMMUL
+  #define mult_32x32_keep32_R(a, x, y) \
+  a = (q31_t) (((q63_t) x * y ) >> 32)
+
+//Enter low optimization region - place directly above function definition
+  #define LOW_OPTIMIZATION_ENTER \
+     _Pragma ("optimize=low")
+
+//Exit low optimization region - place directly after end of function definition
+  #define LOW_OPTIMIZATION_EXIT
+
+//Enter low optimization region - place directly above function definition
+  #define IAR_ONLY_LOW_OPTIMIZATION_ENTER \
+     _Pragma ("optimize=low")
+
+//Exit low optimization region - place directly after end of function definition
+  #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
+
+#elif defined(__GNUC__)
+ //SMMLA
+  #define multAcc_32x32_keep32_R(a, x, y) \
+  a += (q31_t) (((q63_t) x * y) >> 32)
+
+ //SMMLS
+  #define multSub_32x32_keep32_R(a, x, y) \
+  a -= (q31_t) (((q63_t) x * y) >> 32)
+
+//SMMUL
+  #define mult_32x32_keep32_R(a, x, y) \
+  a = (q31_t) (((q63_t) x * y ) >> 32)
+
+  #define LOW_OPTIMIZATION_ENTER __attribute__(( optimize("-O1") ))
+
+  #define LOW_OPTIMIZATION_EXIT
+
+  #define IAR_ONLY_LOW_OPTIMIZATION_ENTER
+
+  #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
+
+#endif
+
+
+
+
+
+#ifdef	__cplusplus
+}
+#endif
+
+
+#endif /* _ARM_MATH_H */
+
+
+/**
+ *
+ * End of file.
+ */
diff --git a/src/lib/mathlib/CMSIS/Include/core_cm3.h b/src/lib/mathlib/CMSIS/Include/core_cm3.h
new file mode 100644
index 000000000..8ac6dc078
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Include/core_cm3.h
@@ -0,0 +1,1627 @@
+/**************************************************************************//**
+ * @file     core_cm3.h
+ * @brief    CMSIS Cortex-M3 Core Peripheral Access Layer Header File
+ * @version  V3.20
+ * @date     25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+   All rights reserved.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+   - Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+   ---------------------------------------------------------------------------*/
+
+
+#if defined ( __ICCARM__ )
+ #pragma system_include  /* treat file as system include file for MISRA check */
+#endif
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#ifndef __CORE_CM3_H_GENERIC
+#define __CORE_CM3_H_GENERIC
+
+/** \page CMSIS_MISRA_Exceptions  MISRA-C:2004 Compliance Exceptions
+  CMSIS violates the following MISRA-C:2004 rules:
+
+   \li Required Rule 8.5, object/function definition in header file.<br>
+     Function definitions in header files are used to allow 'inlining'.
+
+   \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
+     Unions are used for effective representation of core registers.
+
+   \li Advisory Rule 19.7, Function-like macro defined.<br>
+     Function-like macros are used to allow more efficient code.
+ */
+
+
+/*******************************************************************************
+ *                 CMSIS definitions
+ ******************************************************************************/
+/** \ingroup Cortex_M3
+  @{
+ */
+
+/*  CMSIS CM3 definitions */
+#define __CM3_CMSIS_VERSION_MAIN  (0x03)                                   /*!< [31:16] CMSIS HAL main version   */
+#define __CM3_CMSIS_VERSION_SUB   (0x20)                                   /*!< [15:0]  CMSIS HAL sub version    */
+#define __CM3_CMSIS_VERSION       ((__CM3_CMSIS_VERSION_MAIN << 16) | \
+                                    __CM3_CMSIS_VERSION_SUB          )     /*!< CMSIS HAL version number         */
+
+#define __CORTEX_M                (0x03)                                   /*!< Cortex-M Core                    */
+
+
+#if   defined ( __CC_ARM )
+  #define __ASM            __asm                                      /*!< asm keyword for ARM Compiler          */
+  #define __INLINE         __inline                                   /*!< inline keyword for ARM Compiler       */
+  #define __STATIC_INLINE  static __inline
+
+#elif defined ( __ICCARM__ )
+  #define __ASM            __asm                                      /*!< asm keyword for IAR Compiler          */
+  #define __INLINE         inline                                     /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */
+  #define __STATIC_INLINE  static inline
+
+#elif defined ( __TMS470__ )
+  #define __ASM            __asm                                      /*!< asm keyword for TI CCS Compiler       */
+  #define __STATIC_INLINE  static inline
+
+#elif defined ( __GNUC__ )
+  #define __ASM            __asm                                      /*!< asm keyword for GNU Compiler          */
+  #define __INLINE         inline                                     /*!< inline keyword for GNU Compiler       */
+  #define __STATIC_INLINE  static inline
+
+#elif defined ( __TASKING__ )
+  #define __ASM            __asm                                      /*!< asm keyword for TASKING Compiler      */
+  #define __INLINE         inline                                     /*!< inline keyword for TASKING Compiler   */
+  #define __STATIC_INLINE  static inline
+
+#endif
+
+/** __FPU_USED indicates whether an FPU is used or not. This core does not support an FPU at all
+*/
+#define __FPU_USED       0
+
+#if defined ( __CC_ARM )
+  #if defined __TARGET_FPU_VFP
+    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __ICCARM__ )
+  #if defined __ARMVFP__
+    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __TMS470__ )
+  #if defined __TI__VFP_SUPPORT____
+    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __GNUC__ )
+  #if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __TASKING__ )
+  #if defined __FPU_VFP__
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+#endif
+
+#include <stdint.h>                      /* standard types definitions                      */
+#include "core_cmInstr.h"                /* Core Instruction Access                         */
+#include "core_cmFunc.h"                 /* Core Function Access                            */
+
+#endif /* __CORE_CM3_H_GENERIC */
+
+#ifndef __CMSIS_GENERIC
+
+#ifndef __CORE_CM3_H_DEPENDANT
+#define __CORE_CM3_H_DEPENDANT
+
+/* check device defines and use defaults */
+#if defined __CHECK_DEVICE_DEFINES
+  #ifndef __CM3_REV
+    #define __CM3_REV               0x0200
+    #warning "__CM3_REV not defined in device header file; using default!"
+  #endif
+
+  #ifndef __MPU_PRESENT
+    #define __MPU_PRESENT             0
+    #warning "__MPU_PRESENT not defined in device header file; using default!"
+  #endif
+
+  #ifndef __NVIC_PRIO_BITS
+    #define __NVIC_PRIO_BITS          4
+    #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
+  #endif
+
+  #ifndef __Vendor_SysTickConfig
+    #define __Vendor_SysTickConfig    0
+    #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
+  #endif
+#endif
+
+/* IO definitions (access restrictions to peripheral registers) */
+/**
+    \defgroup CMSIS_glob_defs CMSIS Global Defines
+
+    <strong>IO Type Qualifiers</strong> are used
+    \li to specify the access to peripheral variables.
+    \li for automatic generation of peripheral register debug information.
+*/
+#ifdef __cplusplus
+  #define   __I     volatile             /*!< Defines 'read only' permissions                 */
+#else
+  #define   __I     volatile const       /*!< Defines 'read only' permissions                 */
+#endif
+#define     __O     volatile             /*!< Defines 'write only' permissions                */
+#define     __IO    volatile             /*!< Defines 'read / write' permissions              */
+
+/*@} end of group Cortex_M3 */
+
+
+
+/*******************************************************************************
+ *                 Register Abstraction
+  Core Register contain:
+  - Core Register
+  - Core NVIC Register
+  - Core SCB Register
+  - Core SysTick Register
+  - Core Debug Register
+  - Core MPU Register
+ ******************************************************************************/
+/** \defgroup CMSIS_core_register Defines and Type Definitions
+    \brief Type definitions and defines for Cortex-M processor based devices.
+*/
+
+/** \ingroup    CMSIS_core_register
+    \defgroup   CMSIS_CORE  Status and Control Registers
+    \brief  Core Register type definitions.
+  @{
+ */
+
+/** \brief  Union type to access the Application Program Status Register (APSR).
+ */
+typedef union
+{
+  struct
+  {
+#if (__CORTEX_M != 0x04)
+    uint32_t _reserved0:27;              /*!< bit:  0..26  Reserved                           */
+#else
+    uint32_t _reserved0:16;              /*!< bit:  0..15  Reserved                           */
+    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */
+    uint32_t _reserved1:7;               /*!< bit: 20..26  Reserved                           */
+#endif
+    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */
+    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */
+    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */
+    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */
+    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */
+  } b;                                   /*!< Structure used for bit  access                  */
+  uint32_t w;                            /*!< Type      used for word access                  */
+} APSR_Type;
+
+
+/** \brief  Union type to access the Interrupt Program Status Register (IPSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */
+    uint32_t _reserved0:23;              /*!< bit:  9..31  Reserved                           */
+  } b;                                   /*!< Structure used for bit  access                  */
+  uint32_t w;                            /*!< Type      used for word access                  */
+} IPSR_Type;
+
+
+/** \brief  Union type to access the Special-Purpose Program Status Registers (xPSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */
+#if (__CORTEX_M != 0x04)
+    uint32_t _reserved0:15;              /*!< bit:  9..23  Reserved                           */
+#else
+    uint32_t _reserved0:7;               /*!< bit:  9..15  Reserved                           */
+    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */
+    uint32_t _reserved1:4;               /*!< bit: 20..23  Reserved                           */
+#endif
+    uint32_t T:1;                        /*!< bit:     24  Thumb bit        (read 0)          */
+    uint32_t IT:2;                       /*!< bit: 25..26  saved IT state   (read 0)          */
+    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */
+    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */
+    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */
+    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */
+    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */
+  } b;                                   /*!< Structure used for bit  access                  */
+  uint32_t w;                            /*!< Type      used for word access                  */
+} xPSR_Type;
+
+
+/** \brief  Union type to access the Control Registers (CONTROL).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t nPRIV:1;                    /*!< bit:      0  Execution privilege in Thread mode */
+    uint32_t SPSEL:1;                    /*!< bit:      1  Stack to be used                   */
+    uint32_t FPCA:1;                     /*!< bit:      2  FP extension active flag           */
+    uint32_t _reserved0:29;              /*!< bit:  3..31  Reserved                           */
+  } b;                                   /*!< Structure used for bit  access                  */
+  uint32_t w;                            /*!< Type      used for word access                  */
+} CONTROL_Type;
+
+/*@} end of group CMSIS_CORE */
+
+
+/** \ingroup    CMSIS_core_register
+    \defgroup   CMSIS_NVIC  Nested Vectored Interrupt Controller (NVIC)
+    \brief      Type definitions for the NVIC Registers
+  @{
+ */
+
+/** \brief  Structure type to access the Nested Vectored Interrupt Controller (NVIC).
+ */
+typedef struct
+{
+  __IO uint32_t ISER[8];                 /*!< Offset: 0x000 (R/W)  Interrupt Set Enable Register           */
+       uint32_t RESERVED0[24];
+  __IO uint32_t ICER[8];                 /*!< Offset: 0x080 (R/W)  Interrupt Clear Enable Register         */
+       uint32_t RSERVED1[24];
+  __IO uint32_t ISPR[8];                 /*!< Offset: 0x100 (R/W)  Interrupt Set Pending Register          */
+       uint32_t RESERVED2[24];
+  __IO uint32_t ICPR[8];                 /*!< Offset: 0x180 (R/W)  Interrupt Clear Pending Register        */
+       uint32_t RESERVED3[24];
+  __IO uint32_t IABR[8];                 /*!< Offset: 0x200 (R/W)  Interrupt Active bit Register           */
+       uint32_t RESERVED4[56];
+  __IO uint8_t  IP[240];                 /*!< Offset: 0x300 (R/W)  Interrupt Priority Register (8Bit wide) */
+       uint32_t RESERVED5[644];
+  __O  uint32_t STIR;                    /*!< Offset: 0xE00 ( /W)  Software Trigger Interrupt Register     */
+}  NVIC_Type;
+
+/* Software Triggered Interrupt Register Definitions */
+#define NVIC_STIR_INTID_Pos                 0                                          /*!< STIR: INTLINESNUM Position */
+#define NVIC_STIR_INTID_Msk                (0x1FFUL << NVIC_STIR_INTID_Pos)            /*!< STIR: INTLINESNUM Mask */
+
+/*@} end of group CMSIS_NVIC */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_SCB     System Control Block (SCB)
+    \brief      Type definitions for the System Control Block Registers
+  @{
+ */
+
+/** \brief  Structure type to access the System Control Block (SCB).
+ */
+typedef struct
+{
+  __I  uint32_t CPUID;                   /*!< Offset: 0x000 (R/ )  CPUID Base Register                                   */
+  __IO uint32_t ICSR;                    /*!< Offset: 0x004 (R/W)  Interrupt Control and State Register                  */
+  __IO uint32_t VTOR;                    /*!< Offset: 0x008 (R/W)  Vector Table Offset Register                          */
+  __IO uint32_t AIRCR;                   /*!< Offset: 0x00C (R/W)  Application Interrupt and Reset Control Register      */
+  __IO uint32_t SCR;                     /*!< Offset: 0x010 (R/W)  System Control Register                               */
+  __IO uint32_t CCR;                     /*!< Offset: 0x014 (R/W)  Configuration Control Register                        */
+  __IO uint8_t  SHP[12];                 /*!< Offset: 0x018 (R/W)  System Handlers Priority Registers (4-7, 8-11, 12-15) */
+  __IO uint32_t SHCSR;                   /*!< Offset: 0x024 (R/W)  System Handler Control and State Register             */
+  __IO uint32_t CFSR;                    /*!< Offset: 0x028 (R/W)  Configurable Fault Status Register                    */
+  __IO uint32_t HFSR;                    /*!< Offset: 0x02C (R/W)  HardFault Status Register                             */
+  __IO uint32_t DFSR;                    /*!< Offset: 0x030 (R/W)  Debug Fault Status Register                           */
+  __IO uint32_t MMFAR;                   /*!< Offset: 0x034 (R/W)  MemManage Fault Address Register                      */
+  __IO uint32_t BFAR;                    /*!< Offset: 0x038 (R/W)  BusFault Address Register                             */
+  __IO uint32_t AFSR;                    /*!< Offset: 0x03C (R/W)  Auxiliary Fault Status Register                       */
+  __I  uint32_t PFR[2];                  /*!< Offset: 0x040 (R/ )  Processor Feature Register                            */
+  __I  uint32_t DFR;                     /*!< Offset: 0x048 (R/ )  Debug Feature Register                                */
+  __I  uint32_t ADR;                     /*!< Offset: 0x04C (R/ )  Auxiliary Feature Register                            */
+  __I  uint32_t MMFR[4];                 /*!< Offset: 0x050 (R/ )  Memory Model Feature Register                         */
+  __I  uint32_t ISAR[5];                 /*!< Offset: 0x060 (R/ )  Instruction Set Attributes Register                   */
+       uint32_t RESERVED0[5];
+  __IO uint32_t CPACR;                   /*!< Offset: 0x088 (R/W)  Coprocessor Access Control Register                   */
+} SCB_Type;
+
+/* SCB CPUID Register Definitions */
+#define SCB_CPUID_IMPLEMENTER_Pos          24                                             /*!< SCB CPUID: IMPLEMENTER Position */
+#define SCB_CPUID_IMPLEMENTER_Msk          (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos)          /*!< SCB CPUID: IMPLEMENTER Mask */
+
+#define SCB_CPUID_VARIANT_Pos              20                                             /*!< SCB CPUID: VARIANT Position */
+#define SCB_CPUID_VARIANT_Msk              (0xFUL << SCB_CPUID_VARIANT_Pos)               /*!< SCB CPUID: VARIANT Mask */
+
+#define SCB_CPUID_ARCHITECTURE_Pos         16                                             /*!< SCB CPUID: ARCHITECTURE Position */
+#define SCB_CPUID_ARCHITECTURE_Msk         (0xFUL << SCB_CPUID_ARCHITECTURE_Pos)          /*!< SCB CPUID: ARCHITECTURE Mask */
+
+#define SCB_CPUID_PARTNO_Pos                4                                             /*!< SCB CPUID: PARTNO Position */
+#define SCB_CPUID_PARTNO_Msk               (0xFFFUL << SCB_CPUID_PARTNO_Pos)              /*!< SCB CPUID: PARTNO Mask */
+
+#define SCB_CPUID_REVISION_Pos              0                                             /*!< SCB CPUID: REVISION Position */
+#define SCB_CPUID_REVISION_Msk             (0xFUL << SCB_CPUID_REVISION_Pos)              /*!< SCB CPUID: REVISION Mask */
+
+/* SCB Interrupt Control State Register Definitions */
+#define SCB_ICSR_NMIPENDSET_Pos            31                                             /*!< SCB ICSR: NMIPENDSET Position */
+#define SCB_ICSR_NMIPENDSET_Msk            (1UL << SCB_ICSR_NMIPENDSET_Pos)               /*!< SCB ICSR: NMIPENDSET Mask */
+
+#define SCB_ICSR_PENDSVSET_Pos             28                                             /*!< SCB ICSR: PENDSVSET Position */
+#define SCB_ICSR_PENDSVSET_Msk             (1UL << SCB_ICSR_PENDSVSET_Pos)                /*!< SCB ICSR: PENDSVSET Mask */
+
+#define SCB_ICSR_PENDSVCLR_Pos             27                                             /*!< SCB ICSR: PENDSVCLR Position */
+#define SCB_ICSR_PENDSVCLR_Msk             (1UL << SCB_ICSR_PENDSVCLR_Pos)                /*!< SCB ICSR: PENDSVCLR Mask */
+
+#define SCB_ICSR_PENDSTSET_Pos             26                                             /*!< SCB ICSR: PENDSTSET Position */
+#define SCB_ICSR_PENDSTSET_Msk             (1UL << SCB_ICSR_PENDSTSET_Pos)                /*!< SCB ICSR: PENDSTSET Mask */
+
+#define SCB_ICSR_PENDSTCLR_Pos             25                                             /*!< SCB ICSR: PENDSTCLR Position */
+#define SCB_ICSR_PENDSTCLR_Msk             (1UL << SCB_ICSR_PENDSTCLR_Pos)                /*!< SCB ICSR: PENDSTCLR Mask */
+
+#define SCB_ICSR_ISRPREEMPT_Pos            23                                             /*!< SCB ICSR: ISRPREEMPT Position */
+#define SCB_ICSR_ISRPREEMPT_Msk            (1UL << SCB_ICSR_ISRPREEMPT_Pos)               /*!< SCB ICSR: ISRPREEMPT Mask */
+
+#define SCB_ICSR_ISRPENDING_Pos            22                                             /*!< SCB ICSR: ISRPENDING Position */
+#define SCB_ICSR_ISRPENDING_Msk            (1UL << SCB_ICSR_ISRPENDING_Pos)               /*!< SCB ICSR: ISRPENDING Mask */
+
+#define SCB_ICSR_VECTPENDING_Pos           12                                             /*!< SCB ICSR: VECTPENDING Position */
+#define SCB_ICSR_VECTPENDING_Msk           (0x1FFUL << SCB_ICSR_VECTPENDING_Pos)          /*!< SCB ICSR: VECTPENDING Mask */
+
+#define SCB_ICSR_RETTOBASE_Pos             11                                             /*!< SCB ICSR: RETTOBASE Position */
+#define SCB_ICSR_RETTOBASE_Msk             (1UL << SCB_ICSR_RETTOBASE_Pos)                /*!< SCB ICSR: RETTOBASE Mask */
+
+#define SCB_ICSR_VECTACTIVE_Pos             0                                             /*!< SCB ICSR: VECTACTIVE Position */
+#define SCB_ICSR_VECTACTIVE_Msk            (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos)           /*!< SCB ICSR: VECTACTIVE Mask */
+
+/* SCB Vector Table Offset Register Definitions */
+#if (__CM3_REV < 0x0201)                   /* core r2p1 */
+#define SCB_VTOR_TBLBASE_Pos               29                                             /*!< SCB VTOR: TBLBASE Position */
+#define SCB_VTOR_TBLBASE_Msk               (1UL << SCB_VTOR_TBLBASE_Pos)                  /*!< SCB VTOR: TBLBASE Mask */
+
+#define SCB_VTOR_TBLOFF_Pos                 7                                             /*!< SCB VTOR: TBLOFF Position */
+#define SCB_VTOR_TBLOFF_Msk                (0x3FFFFFUL << SCB_VTOR_TBLOFF_Pos)            /*!< SCB VTOR: TBLOFF Mask */
+#else
+#define SCB_VTOR_TBLOFF_Pos                 7                                             /*!< SCB VTOR: TBLOFF Position */
+#define SCB_VTOR_TBLOFF_Msk                (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos)           /*!< SCB VTOR: TBLOFF Mask */
+#endif
+
+/* SCB Application Interrupt and Reset Control Register Definitions */
+#define SCB_AIRCR_VECTKEY_Pos              16                                             /*!< SCB AIRCR: VECTKEY Position */
+#define SCB_AIRCR_VECTKEY_Msk              (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos)            /*!< SCB AIRCR: VECTKEY Mask */
+
+#define SCB_AIRCR_VECTKEYSTAT_Pos          16                                             /*!< SCB AIRCR: VECTKEYSTAT Position */
+#define SCB_AIRCR_VECTKEYSTAT_Msk          (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos)        /*!< SCB AIRCR: VECTKEYSTAT Mask */
+
+#define SCB_AIRCR_ENDIANESS_Pos            15                                             /*!< SCB AIRCR: ENDIANESS Position */
+#define SCB_AIRCR_ENDIANESS_Msk            (1UL << SCB_AIRCR_ENDIANESS_Pos)               /*!< SCB AIRCR: ENDIANESS Mask */
+
+#define SCB_AIRCR_PRIGROUP_Pos              8                                             /*!< SCB AIRCR: PRIGROUP Position */
+#define SCB_AIRCR_PRIGROUP_Msk             (7UL << SCB_AIRCR_PRIGROUP_Pos)                /*!< SCB AIRCR: PRIGROUP Mask */
+
+#define SCB_AIRCR_SYSRESETREQ_Pos           2                                             /*!< SCB AIRCR: SYSRESETREQ Position */
+#define SCB_AIRCR_SYSRESETREQ_Msk          (1UL << SCB_AIRCR_SYSRESETREQ_Pos)             /*!< SCB AIRCR: SYSRESETREQ Mask */
+
+#define SCB_AIRCR_VECTCLRACTIVE_Pos         1                                             /*!< SCB AIRCR: VECTCLRACTIVE Position */
+#define SCB_AIRCR_VECTCLRACTIVE_Msk        (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos)           /*!< SCB AIRCR: VECTCLRACTIVE Mask */
+
+#define SCB_AIRCR_VECTRESET_Pos             0                                             /*!< SCB AIRCR: VECTRESET Position */
+#define SCB_AIRCR_VECTRESET_Msk            (1UL << SCB_AIRCR_VECTRESET_Pos)               /*!< SCB AIRCR: VECTRESET Mask */
+
+/* SCB System Control Register Definitions */
+#define SCB_SCR_SEVONPEND_Pos               4                                             /*!< SCB SCR: SEVONPEND Position */
+#define SCB_SCR_SEVONPEND_Msk              (1UL << SCB_SCR_SEVONPEND_Pos)                 /*!< SCB SCR: SEVONPEND Mask */
+
+#define SCB_SCR_SLEEPDEEP_Pos               2                                             /*!< SCB SCR: SLEEPDEEP Position */
+#define SCB_SCR_SLEEPDEEP_Msk              (1UL << SCB_SCR_SLEEPDEEP_Pos)                 /*!< SCB SCR: SLEEPDEEP Mask */
+
+#define SCB_SCR_SLEEPONEXIT_Pos             1                                             /*!< SCB SCR: SLEEPONEXIT Position */
+#define SCB_SCR_SLEEPONEXIT_Msk            (1UL << SCB_SCR_SLEEPONEXIT_Pos)               /*!< SCB SCR: SLEEPONEXIT Mask */
+
+/* SCB Configuration Control Register Definitions */
+#define SCB_CCR_STKALIGN_Pos                9                                             /*!< SCB CCR: STKALIGN Position */
+#define SCB_CCR_STKALIGN_Msk               (1UL << SCB_CCR_STKALIGN_Pos)                  /*!< SCB CCR: STKALIGN Mask */
+
+#define SCB_CCR_BFHFNMIGN_Pos               8                                             /*!< SCB CCR: BFHFNMIGN Position */
+#define SCB_CCR_BFHFNMIGN_Msk              (1UL << SCB_CCR_BFHFNMIGN_Pos)                 /*!< SCB CCR: BFHFNMIGN Mask */
+
+#define SCB_CCR_DIV_0_TRP_Pos               4                                             /*!< SCB CCR: DIV_0_TRP Position */
+#define SCB_CCR_DIV_0_TRP_Msk              (1UL << SCB_CCR_DIV_0_TRP_Pos)                 /*!< SCB CCR: DIV_0_TRP Mask */
+
+#define SCB_CCR_UNALIGN_TRP_Pos             3                                             /*!< SCB CCR: UNALIGN_TRP Position */
+#define SCB_CCR_UNALIGN_TRP_Msk            (1UL << SCB_CCR_UNALIGN_TRP_Pos)               /*!< SCB CCR: UNALIGN_TRP Mask */
+
+#define SCB_CCR_USERSETMPEND_Pos            1                                             /*!< SCB CCR: USERSETMPEND Position */
+#define SCB_CCR_USERSETMPEND_Msk           (1UL << SCB_CCR_USERSETMPEND_Pos)              /*!< SCB CCR: USERSETMPEND Mask */
+
+#define SCB_CCR_NONBASETHRDENA_Pos          0                                             /*!< SCB CCR: NONBASETHRDENA Position */
+#define SCB_CCR_NONBASETHRDENA_Msk         (1UL << SCB_CCR_NONBASETHRDENA_Pos)            /*!< SCB CCR: NONBASETHRDENA Mask */
+
+/* SCB System Handler Control and State Register Definitions */
+#define SCB_SHCSR_USGFAULTENA_Pos          18                                             /*!< SCB SHCSR: USGFAULTENA Position */
+#define SCB_SHCSR_USGFAULTENA_Msk          (1UL << SCB_SHCSR_USGFAULTENA_Pos)             /*!< SCB SHCSR: USGFAULTENA Mask */
+
+#define SCB_SHCSR_BUSFAULTENA_Pos          17                                             /*!< SCB SHCSR: BUSFAULTENA Position */
+#define SCB_SHCSR_BUSFAULTENA_Msk          (1UL << SCB_SHCSR_BUSFAULTENA_Pos)             /*!< SCB SHCSR: BUSFAULTENA Mask */
+
+#define SCB_SHCSR_MEMFAULTENA_Pos          16                                             /*!< SCB SHCSR: MEMFAULTENA Position */
+#define SCB_SHCSR_MEMFAULTENA_Msk          (1UL << SCB_SHCSR_MEMFAULTENA_Pos)             /*!< SCB SHCSR: MEMFAULTENA Mask */
+
+#define SCB_SHCSR_SVCALLPENDED_Pos         15                                             /*!< SCB SHCSR: SVCALLPENDED Position */
+#define SCB_SHCSR_SVCALLPENDED_Msk         (1UL << SCB_SHCSR_SVCALLPENDED_Pos)            /*!< SCB SHCSR: SVCALLPENDED Mask */
+
+#define SCB_SHCSR_BUSFAULTPENDED_Pos       14                                             /*!< SCB SHCSR: BUSFAULTPENDED Position */
+#define SCB_SHCSR_BUSFAULTPENDED_Msk       (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos)          /*!< SCB SHCSR: BUSFAULTPENDED Mask */
+
+#define SCB_SHCSR_MEMFAULTPENDED_Pos       13                                             /*!< SCB SHCSR: MEMFAULTPENDED Position */
+#define SCB_SHCSR_MEMFAULTPENDED_Msk       (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos)          /*!< SCB SHCSR: MEMFAULTPENDED Mask */
+
+#define SCB_SHCSR_USGFAULTPENDED_Pos       12                                             /*!< SCB SHCSR: USGFAULTPENDED Position */
+#define SCB_SHCSR_USGFAULTPENDED_Msk       (1UL << SCB_SHCSR_USGFAULTPENDED_Pos)          /*!< SCB SHCSR: USGFAULTPENDED Mask */
+
+#define SCB_SHCSR_SYSTICKACT_Pos           11                                             /*!< SCB SHCSR: SYSTICKACT Position */
+#define SCB_SHCSR_SYSTICKACT_Msk           (1UL << SCB_SHCSR_SYSTICKACT_Pos)              /*!< SCB SHCSR: SYSTICKACT Mask */
+
+#define SCB_SHCSR_PENDSVACT_Pos            10                                             /*!< SCB SHCSR: PENDSVACT Position */
+#define SCB_SHCSR_PENDSVACT_Msk            (1UL << SCB_SHCSR_PENDSVACT_Pos)               /*!< SCB SHCSR: PENDSVACT Mask */
+
+#define SCB_SHCSR_MONITORACT_Pos            8                                             /*!< SCB SHCSR: MONITORACT Position */
+#define SCB_SHCSR_MONITORACT_Msk           (1UL << SCB_SHCSR_MONITORACT_Pos)              /*!< SCB SHCSR: MONITORACT Mask */
+
+#define SCB_SHCSR_SVCALLACT_Pos             7                                             /*!< SCB SHCSR: SVCALLACT Position */
+#define SCB_SHCSR_SVCALLACT_Msk            (1UL << SCB_SHCSR_SVCALLACT_Pos)               /*!< SCB SHCSR: SVCALLACT Mask */
+
+#define SCB_SHCSR_USGFAULTACT_Pos           3                                             /*!< SCB SHCSR: USGFAULTACT Position */
+#define SCB_SHCSR_USGFAULTACT_Msk          (1UL << SCB_SHCSR_USGFAULTACT_Pos)             /*!< SCB SHCSR: USGFAULTACT Mask */
+
+#define SCB_SHCSR_BUSFAULTACT_Pos           1                                             /*!< SCB SHCSR: BUSFAULTACT Position */
+#define SCB_SHCSR_BUSFAULTACT_Msk          (1UL << SCB_SHCSR_BUSFAULTACT_Pos)             /*!< SCB SHCSR: BUSFAULTACT Mask */
+
+#define SCB_SHCSR_MEMFAULTACT_Pos           0                                             /*!< SCB SHCSR: MEMFAULTACT Position */
+#define SCB_SHCSR_MEMFAULTACT_Msk          (1UL << SCB_SHCSR_MEMFAULTACT_Pos)             /*!< SCB SHCSR: MEMFAULTACT Mask */
+
+/* SCB Configurable Fault Status Registers Definitions */
+#define SCB_CFSR_USGFAULTSR_Pos            16                                             /*!< SCB CFSR: Usage Fault Status Register Position */
+#define SCB_CFSR_USGFAULTSR_Msk            (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos)          /*!< SCB CFSR: Usage Fault Status Register Mask */
+
+#define SCB_CFSR_BUSFAULTSR_Pos             8                                             /*!< SCB CFSR: Bus Fault Status Register Position */
+#define SCB_CFSR_BUSFAULTSR_Msk            (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos)            /*!< SCB CFSR: Bus Fault Status Register Mask */
+
+#define SCB_CFSR_MEMFAULTSR_Pos             0                                             /*!< SCB CFSR: Memory Manage Fault Status Register Position */
+#define SCB_CFSR_MEMFAULTSR_Msk            (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos)            /*!< SCB CFSR: Memory Manage Fault Status Register Mask */
+
+/* SCB Hard Fault Status Registers Definitions */
+#define SCB_HFSR_DEBUGEVT_Pos              31                                             /*!< SCB HFSR: DEBUGEVT Position */
+#define SCB_HFSR_DEBUGEVT_Msk              (1UL << SCB_HFSR_DEBUGEVT_Pos)                 /*!< SCB HFSR: DEBUGEVT Mask */
+
+#define SCB_HFSR_FORCED_Pos                30                                             /*!< SCB HFSR: FORCED Position */
+#define SCB_HFSR_FORCED_Msk                (1UL << SCB_HFSR_FORCED_Pos)                   /*!< SCB HFSR: FORCED Mask */
+
+#define SCB_HFSR_VECTTBL_Pos                1                                             /*!< SCB HFSR: VECTTBL Position */
+#define SCB_HFSR_VECTTBL_Msk               (1UL << SCB_HFSR_VECTTBL_Pos)                  /*!< SCB HFSR: VECTTBL Mask */
+
+/* SCB Debug Fault Status Register Definitions */
+#define SCB_DFSR_EXTERNAL_Pos               4                                             /*!< SCB DFSR: EXTERNAL Position */
+#define SCB_DFSR_EXTERNAL_Msk              (1UL << SCB_DFSR_EXTERNAL_Pos)                 /*!< SCB DFSR: EXTERNAL Mask */
+
+#define SCB_DFSR_VCATCH_Pos                 3                                             /*!< SCB DFSR: VCATCH Position */
+#define SCB_DFSR_VCATCH_Msk                (1UL << SCB_DFSR_VCATCH_Pos)                   /*!< SCB DFSR: VCATCH Mask */
+
+#define SCB_DFSR_DWTTRAP_Pos                2                                             /*!< SCB DFSR: DWTTRAP Position */
+#define SCB_DFSR_DWTTRAP_Msk               (1UL << SCB_DFSR_DWTTRAP_Pos)                  /*!< SCB DFSR: DWTTRAP Mask */
+
+#define SCB_DFSR_BKPT_Pos                   1                                             /*!< SCB DFSR: BKPT Position */
+#define SCB_DFSR_BKPT_Msk                  (1UL << SCB_DFSR_BKPT_Pos)                     /*!< SCB DFSR: BKPT Mask */
+
+#define SCB_DFSR_HALTED_Pos                 0                                             /*!< SCB DFSR: HALTED Position */
+#define SCB_DFSR_HALTED_Msk                (1UL << SCB_DFSR_HALTED_Pos)                   /*!< SCB DFSR: HALTED Mask */
+
+/*@} end of group CMSIS_SCB */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)
+    \brief      Type definitions for the System Control and ID Register not in the SCB
+  @{
+ */
+
+/** \brief  Structure type to access the System Control and ID Register not in the SCB.
+ */
+typedef struct
+{
+       uint32_t RESERVED0[1];
+  __I  uint32_t ICTR;                    /*!< Offset: 0x004 (R/ )  Interrupt Controller Type Register      */
+#if ((defined __CM3_REV) && (__CM3_REV >= 0x200))
+  __IO uint32_t ACTLR;                   /*!< Offset: 0x008 (R/W)  Auxiliary Control Register      */
+#else
+       uint32_t RESERVED1[1];
+#endif
+} SCnSCB_Type;
+
+/* Interrupt Controller Type Register Definitions */
+#define SCnSCB_ICTR_INTLINESNUM_Pos         0                                          /*!< ICTR: INTLINESNUM Position */
+#define SCnSCB_ICTR_INTLINESNUM_Msk        (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos)      /*!< ICTR: INTLINESNUM Mask */
+
+/* Auxiliary Control Register Definitions */
+
+#define SCnSCB_ACTLR_DISFOLD_Pos            2                                          /*!< ACTLR: DISFOLD Position */
+#define SCnSCB_ACTLR_DISFOLD_Msk           (1UL << SCnSCB_ACTLR_DISFOLD_Pos)           /*!< ACTLR: DISFOLD Mask */
+
+#define SCnSCB_ACTLR_DISDEFWBUF_Pos         1                                          /*!< ACTLR: DISDEFWBUF Position */
+#define SCnSCB_ACTLR_DISDEFWBUF_Msk        (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos)        /*!< ACTLR: DISDEFWBUF Mask */
+
+#define SCnSCB_ACTLR_DISMCYCINT_Pos         0                                          /*!< ACTLR: DISMCYCINT Position */
+#define SCnSCB_ACTLR_DISMCYCINT_Msk        (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos)        /*!< ACTLR: DISMCYCINT Mask */
+
+/*@} end of group CMSIS_SCnotSCB */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_SysTick     System Tick Timer (SysTick)
+    \brief      Type definitions for the System Timer Registers.
+  @{
+ */
+
+/** \brief  Structure type to access the System Timer (SysTick).
+ */
+typedef struct
+{
+  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  SysTick Control and Status Register */
+  __IO uint32_t LOAD;                    /*!< Offset: 0x004 (R/W)  SysTick Reload Value Register       */
+  __IO uint32_t VAL;                     /*!< Offset: 0x008 (R/W)  SysTick Current Value Register      */
+  __I  uint32_t CALIB;                   /*!< Offset: 0x00C (R/ )  SysTick Calibration Register        */
+} SysTick_Type;
+
+/* SysTick Control / Status Register Definitions */
+#define SysTick_CTRL_COUNTFLAG_Pos         16                                             /*!< SysTick CTRL: COUNTFLAG Position */
+#define SysTick_CTRL_COUNTFLAG_Msk         (1UL << SysTick_CTRL_COUNTFLAG_Pos)            /*!< SysTick CTRL: COUNTFLAG Mask */
+
+#define SysTick_CTRL_CLKSOURCE_Pos          2                                             /*!< SysTick CTRL: CLKSOURCE Position */
+#define SysTick_CTRL_CLKSOURCE_Msk         (1UL << SysTick_CTRL_CLKSOURCE_Pos)            /*!< SysTick CTRL: CLKSOURCE Mask */
+
+#define SysTick_CTRL_TICKINT_Pos            1                                             /*!< SysTick CTRL: TICKINT Position */
+#define SysTick_CTRL_TICKINT_Msk           (1UL << SysTick_CTRL_TICKINT_Pos)              /*!< SysTick CTRL: TICKINT Mask */
+
+#define SysTick_CTRL_ENABLE_Pos             0                                             /*!< SysTick CTRL: ENABLE Position */
+#define SysTick_CTRL_ENABLE_Msk            (1UL << SysTick_CTRL_ENABLE_Pos)               /*!< SysTick CTRL: ENABLE Mask */
+
+/* SysTick Reload Register Definitions */
+#define SysTick_LOAD_RELOAD_Pos             0                                             /*!< SysTick LOAD: RELOAD Position */
+#define SysTick_LOAD_RELOAD_Msk            (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos)        /*!< SysTick LOAD: RELOAD Mask */
+
+/* SysTick Current Register Definitions */
+#define SysTick_VAL_CURRENT_Pos             0                                             /*!< SysTick VAL: CURRENT Position */
+#define SysTick_VAL_CURRENT_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick VAL: CURRENT Mask */
+
+/* SysTick Calibration Register Definitions */
+#define SysTick_CALIB_NOREF_Pos            31                                             /*!< SysTick CALIB: NOREF Position */
+#define SysTick_CALIB_NOREF_Msk            (1UL << SysTick_CALIB_NOREF_Pos)               /*!< SysTick CALIB: NOREF Mask */
+
+#define SysTick_CALIB_SKEW_Pos             30                                             /*!< SysTick CALIB: SKEW Position */
+#define SysTick_CALIB_SKEW_Msk             (1UL << SysTick_CALIB_SKEW_Pos)                /*!< SysTick CALIB: SKEW Mask */
+
+#define SysTick_CALIB_TENMS_Pos             0                                             /*!< SysTick CALIB: TENMS Position */
+#define SysTick_CALIB_TENMS_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick CALIB: TENMS Mask */
+
+/*@} end of group CMSIS_SysTick */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_ITM     Instrumentation Trace Macrocell (ITM)
+    \brief      Type definitions for the Instrumentation Trace Macrocell (ITM)
+  @{
+ */
+
+/** \brief  Structure type to access the Instrumentation Trace Macrocell Register (ITM).
+ */
+typedef struct
+{
+  __O  union
+  {
+    __O  uint8_t    u8;                  /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 8-bit                   */
+    __O  uint16_t   u16;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 16-bit                  */
+    __O  uint32_t   u32;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 32-bit                  */
+  }  PORT [32];                          /*!< Offset: 0x000 ( /W)  ITM Stimulus Port Registers               */
+       uint32_t RESERVED0[864];
+  __IO uint32_t TER;                     /*!< Offset: 0xE00 (R/W)  ITM Trace Enable Register                 */
+       uint32_t RESERVED1[15];
+  __IO uint32_t TPR;                     /*!< Offset: 0xE40 (R/W)  ITM Trace Privilege Register              */
+       uint32_t RESERVED2[15];
+  __IO uint32_t TCR;                     /*!< Offset: 0xE80 (R/W)  ITM Trace Control Register                */
+       uint32_t RESERVED3[29];
+  __O  uint32_t IWR;                     /*!< Offset: 0xEF8 ( /W)  ITM Integration Write Register            */
+  __I  uint32_t IRR;                     /*!< Offset: 0xEFC (R/ )  ITM Integration Read Register             */
+  __IO uint32_t IMCR;                    /*!< Offset: 0xF00 (R/W)  ITM Integration Mode Control Register     */
+       uint32_t RESERVED4[43];
+  __O  uint32_t LAR;                     /*!< Offset: 0xFB0 ( /W)  ITM Lock Access Register                  */
+  __I  uint32_t LSR;                     /*!< Offset: 0xFB4 (R/ )  ITM Lock Status Register                  */
+       uint32_t RESERVED5[6];
+  __I  uint32_t PID4;                    /*!< Offset: 0xFD0 (R/ )  ITM Peripheral Identification Register #4 */
+  __I  uint32_t PID5;                    /*!< Offset: 0xFD4 (R/ )  ITM Peripheral Identification Register #5 */
+  __I  uint32_t PID6;                    /*!< Offset: 0xFD8 (R/ )  ITM Peripheral Identification Register #6 */
+  __I  uint32_t PID7;                    /*!< Offset: 0xFDC (R/ )  ITM Peripheral Identification Register #7 */
+  __I  uint32_t PID0;                    /*!< Offset: 0xFE0 (R/ )  ITM Peripheral Identification Register #0 */
+  __I  uint32_t PID1;                    /*!< Offset: 0xFE4 (R/ )  ITM Peripheral Identification Register #1 */
+  __I  uint32_t PID2;                    /*!< Offset: 0xFE8 (R/ )  ITM Peripheral Identification Register #2 */
+  __I  uint32_t PID3;                    /*!< Offset: 0xFEC (R/ )  ITM Peripheral Identification Register #3 */
+  __I  uint32_t CID0;                    /*!< Offset: 0xFF0 (R/ )  ITM Component  Identification Register #0 */
+  __I  uint32_t CID1;                    /*!< Offset: 0xFF4 (R/ )  ITM Component  Identification Register #1 */
+  __I  uint32_t CID2;                    /*!< Offset: 0xFF8 (R/ )  ITM Component  Identification Register #2 */
+  __I  uint32_t CID3;                    /*!< Offset: 0xFFC (R/ )  ITM Component  Identification Register #3 */
+} ITM_Type;
+
+/* ITM Trace Privilege Register Definitions */
+#define ITM_TPR_PRIVMASK_Pos                0                                             /*!< ITM TPR: PRIVMASK Position */
+#define ITM_TPR_PRIVMASK_Msk               (0xFUL << ITM_TPR_PRIVMASK_Pos)                /*!< ITM TPR: PRIVMASK Mask */
+
+/* ITM Trace Control Register Definitions */
+#define ITM_TCR_BUSY_Pos                   23                                             /*!< ITM TCR: BUSY Position */
+#define ITM_TCR_BUSY_Msk                   (1UL << ITM_TCR_BUSY_Pos)                      /*!< ITM TCR: BUSY Mask */
+
+#define ITM_TCR_TraceBusID_Pos             16                                             /*!< ITM TCR: ATBID Position */
+#define ITM_TCR_TraceBusID_Msk             (0x7FUL << ITM_TCR_TraceBusID_Pos)             /*!< ITM TCR: ATBID Mask */
+
+#define ITM_TCR_GTSFREQ_Pos                10                                             /*!< ITM TCR: Global timestamp frequency Position */
+#define ITM_TCR_GTSFREQ_Msk                (3UL << ITM_TCR_GTSFREQ_Pos)                   /*!< ITM TCR: Global timestamp frequency Mask */
+
+#define ITM_TCR_TSPrescale_Pos              8                                             /*!< ITM TCR: TSPrescale Position */
+#define ITM_TCR_TSPrescale_Msk             (3UL << ITM_TCR_TSPrescale_Pos)                /*!< ITM TCR: TSPrescale Mask */
+
+#define ITM_TCR_SWOENA_Pos                  4                                             /*!< ITM TCR: SWOENA Position */
+#define ITM_TCR_SWOENA_Msk                 (1UL << ITM_TCR_SWOENA_Pos)                    /*!< ITM TCR: SWOENA Mask */
+
+#define ITM_TCR_DWTENA_Pos                  3                                             /*!< ITM TCR: DWTENA Position */
+#define ITM_TCR_DWTENA_Msk                 (1UL << ITM_TCR_DWTENA_Pos)                    /*!< ITM TCR: DWTENA Mask */
+
+#define ITM_TCR_SYNCENA_Pos                 2                                             /*!< ITM TCR: SYNCENA Position */
+#define ITM_TCR_SYNCENA_Msk                (1UL << ITM_TCR_SYNCENA_Pos)                   /*!< ITM TCR: SYNCENA Mask */
+
+#define ITM_TCR_TSENA_Pos                   1                                             /*!< ITM TCR: TSENA Position */
+#define ITM_TCR_TSENA_Msk                  (1UL << ITM_TCR_TSENA_Pos)                     /*!< ITM TCR: TSENA Mask */
+
+#define ITM_TCR_ITMENA_Pos                  0                                             /*!< ITM TCR: ITM Enable bit Position */
+#define ITM_TCR_ITMENA_Msk                 (1UL << ITM_TCR_ITMENA_Pos)                    /*!< ITM TCR: ITM Enable bit Mask */
+
+/* ITM Integration Write Register Definitions */
+#define ITM_IWR_ATVALIDM_Pos                0                                             /*!< ITM IWR: ATVALIDM Position */
+#define ITM_IWR_ATVALIDM_Msk               (1UL << ITM_IWR_ATVALIDM_Pos)                  /*!< ITM IWR: ATVALIDM Mask */
+
+/* ITM Integration Read Register Definitions */
+#define ITM_IRR_ATREADYM_Pos                0                                             /*!< ITM IRR: ATREADYM Position */
+#define ITM_IRR_ATREADYM_Msk               (1UL << ITM_IRR_ATREADYM_Pos)                  /*!< ITM IRR: ATREADYM Mask */
+
+/* ITM Integration Mode Control Register Definitions */
+#define ITM_IMCR_INTEGRATION_Pos            0                                             /*!< ITM IMCR: INTEGRATION Position */
+#define ITM_IMCR_INTEGRATION_Msk           (1UL << ITM_IMCR_INTEGRATION_Pos)              /*!< ITM IMCR: INTEGRATION Mask */
+
+/* ITM Lock Status Register Definitions */
+#define ITM_LSR_ByteAcc_Pos                 2                                             /*!< ITM LSR: ByteAcc Position */
+#define ITM_LSR_ByteAcc_Msk                (1UL << ITM_LSR_ByteAcc_Pos)                   /*!< ITM LSR: ByteAcc Mask */
+
+#define ITM_LSR_Access_Pos                  1                                             /*!< ITM LSR: Access Position */
+#define ITM_LSR_Access_Msk                 (1UL << ITM_LSR_Access_Pos)                    /*!< ITM LSR: Access Mask */
+
+#define ITM_LSR_Present_Pos                 0                                             /*!< ITM LSR: Present Position */
+#define ITM_LSR_Present_Msk                (1UL << ITM_LSR_Present_Pos)                   /*!< ITM LSR: Present Mask */
+
+/*@}*/ /* end of group CMSIS_ITM */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_DWT     Data Watchpoint and Trace (DWT)
+    \brief      Type definitions for the Data Watchpoint and Trace (DWT)
+  @{
+ */
+
+/** \brief  Structure type to access the Data Watchpoint and Trace Register (DWT).
+ */
+typedef struct
+{
+  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  Control Register                          */
+  __IO uint32_t CYCCNT;                  /*!< Offset: 0x004 (R/W)  Cycle Count Register                      */
+  __IO uint32_t CPICNT;                  /*!< Offset: 0x008 (R/W)  CPI Count Register                        */
+  __IO uint32_t EXCCNT;                  /*!< Offset: 0x00C (R/W)  Exception Overhead Count Register         */
+  __IO uint32_t SLEEPCNT;                /*!< Offset: 0x010 (R/W)  Sleep Count Register                      */
+  __IO uint32_t LSUCNT;                  /*!< Offset: 0x014 (R/W)  LSU Count Register                        */
+  __IO uint32_t FOLDCNT;                 /*!< Offset: 0x018 (R/W)  Folded-instruction Count Register         */
+  __I  uint32_t PCSR;                    /*!< Offset: 0x01C (R/ )  Program Counter Sample Register           */
+  __IO uint32_t COMP0;                   /*!< Offset: 0x020 (R/W)  Comparator Register 0                     */
+  __IO uint32_t MASK0;                   /*!< Offset: 0x024 (R/W)  Mask Register 0                           */
+  __IO uint32_t FUNCTION0;               /*!< Offset: 0x028 (R/W)  Function Register 0                       */
+       uint32_t RESERVED0[1];
+  __IO uint32_t COMP1;                   /*!< Offset: 0x030 (R/W)  Comparator Register 1                     */
+  __IO uint32_t MASK1;                   /*!< Offset: 0x034 (R/W)  Mask Register 1                           */
+  __IO uint32_t FUNCTION1;               /*!< Offset: 0x038 (R/W)  Function Register 1                       */
+       uint32_t RESERVED1[1];
+  __IO uint32_t COMP2;                   /*!< Offset: 0x040 (R/W)  Comparator Register 2                     */
+  __IO uint32_t MASK2;                   /*!< Offset: 0x044 (R/W)  Mask Register 2                           */
+  __IO uint32_t FUNCTION2;               /*!< Offset: 0x048 (R/W)  Function Register 2                       */
+       uint32_t RESERVED2[1];
+  __IO uint32_t COMP3;                   /*!< Offset: 0x050 (R/W)  Comparator Register 3                     */
+  __IO uint32_t MASK3;                   /*!< Offset: 0x054 (R/W)  Mask Register 3                           */
+  __IO uint32_t FUNCTION3;               /*!< Offset: 0x058 (R/W)  Function Register 3                       */
+} DWT_Type;
+
+/* DWT Control Register Definitions */
+#define DWT_CTRL_NUMCOMP_Pos               28                                          /*!< DWT CTRL: NUMCOMP Position */
+#define DWT_CTRL_NUMCOMP_Msk               (0xFUL << DWT_CTRL_NUMCOMP_Pos)             /*!< DWT CTRL: NUMCOMP Mask */
+
+#define DWT_CTRL_NOTRCPKT_Pos              27                                          /*!< DWT CTRL: NOTRCPKT Position */
+#define DWT_CTRL_NOTRCPKT_Msk              (0x1UL << DWT_CTRL_NOTRCPKT_Pos)            /*!< DWT CTRL: NOTRCPKT Mask */
+
+#define DWT_CTRL_NOEXTTRIG_Pos             26                                          /*!< DWT CTRL: NOEXTTRIG Position */
+#define DWT_CTRL_NOEXTTRIG_Msk             (0x1UL << DWT_CTRL_NOEXTTRIG_Pos)           /*!< DWT CTRL: NOEXTTRIG Mask */
+
+#define DWT_CTRL_NOCYCCNT_Pos              25                                          /*!< DWT CTRL: NOCYCCNT Position */
+#define DWT_CTRL_NOCYCCNT_Msk              (0x1UL << DWT_CTRL_NOCYCCNT_Pos)            /*!< DWT CTRL: NOCYCCNT Mask */
+
+#define DWT_CTRL_NOPRFCNT_Pos              24                                          /*!< DWT CTRL: NOPRFCNT Position */
+#define DWT_CTRL_NOPRFCNT_Msk              (0x1UL << DWT_CTRL_NOPRFCNT_Pos)            /*!< DWT CTRL: NOPRFCNT Mask */
+
+#define DWT_CTRL_CYCEVTENA_Pos             22                                          /*!< DWT CTRL: CYCEVTENA Position */
+#define DWT_CTRL_CYCEVTENA_Msk             (0x1UL << DWT_CTRL_CYCEVTENA_Pos)           /*!< DWT CTRL: CYCEVTENA Mask */
+
+#define DWT_CTRL_FOLDEVTENA_Pos            21                                          /*!< DWT CTRL: FOLDEVTENA Position */
+#define DWT_CTRL_FOLDEVTENA_Msk            (0x1UL << DWT_CTRL_FOLDEVTENA_Pos)          /*!< DWT CTRL: FOLDEVTENA Mask */
+
+#define DWT_CTRL_LSUEVTENA_Pos             20                                          /*!< DWT CTRL: LSUEVTENA Position */
+#define DWT_CTRL_LSUEVTENA_Msk             (0x1UL << DWT_CTRL_LSUEVTENA_Pos)           /*!< DWT CTRL: LSUEVTENA Mask */
+
+#define DWT_CTRL_SLEEPEVTENA_Pos           19                                          /*!< DWT CTRL: SLEEPEVTENA Position */
+#define DWT_CTRL_SLEEPEVTENA_Msk           (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos)         /*!< DWT CTRL: SLEEPEVTENA Mask */
+
+#define DWT_CTRL_EXCEVTENA_Pos             18                                          /*!< DWT CTRL: EXCEVTENA Position */
+#define DWT_CTRL_EXCEVTENA_Msk             (0x1UL << DWT_CTRL_EXCEVTENA_Pos)           /*!< DWT CTRL: EXCEVTENA Mask */
+
+#define DWT_CTRL_CPIEVTENA_Pos             17                                          /*!< DWT CTRL: CPIEVTENA Position */
+#define DWT_CTRL_CPIEVTENA_Msk             (0x1UL << DWT_CTRL_CPIEVTENA_Pos)           /*!< DWT CTRL: CPIEVTENA Mask */
+
+#define DWT_CTRL_EXCTRCENA_Pos             16                                          /*!< DWT CTRL: EXCTRCENA Position */
+#define DWT_CTRL_EXCTRCENA_Msk             (0x1UL << DWT_CTRL_EXCTRCENA_Pos)           /*!< DWT CTRL: EXCTRCENA Mask */
+
+#define DWT_CTRL_PCSAMPLENA_Pos            12                                          /*!< DWT CTRL: PCSAMPLENA Position */
+#define DWT_CTRL_PCSAMPLENA_Msk            (0x1UL << DWT_CTRL_PCSAMPLENA_Pos)          /*!< DWT CTRL: PCSAMPLENA Mask */
+
+#define DWT_CTRL_SYNCTAP_Pos               10                                          /*!< DWT CTRL: SYNCTAP Position */
+#define DWT_CTRL_SYNCTAP_Msk               (0x3UL << DWT_CTRL_SYNCTAP_Pos)             /*!< DWT CTRL: SYNCTAP Mask */
+
+#define DWT_CTRL_CYCTAP_Pos                 9                                          /*!< DWT CTRL: CYCTAP Position */
+#define DWT_CTRL_CYCTAP_Msk                (0x1UL << DWT_CTRL_CYCTAP_Pos)              /*!< DWT CTRL: CYCTAP Mask */
+
+#define DWT_CTRL_POSTINIT_Pos               5                                          /*!< DWT CTRL: POSTINIT Position */
+#define DWT_CTRL_POSTINIT_Msk              (0xFUL << DWT_CTRL_POSTINIT_Pos)            /*!< DWT CTRL: POSTINIT Mask */
+
+#define DWT_CTRL_POSTPRESET_Pos             1                                          /*!< DWT CTRL: POSTPRESET Position */
+#define DWT_CTRL_POSTPRESET_Msk            (0xFUL << DWT_CTRL_POSTPRESET_Pos)          /*!< DWT CTRL: POSTPRESET Mask */
+
+#define DWT_CTRL_CYCCNTENA_Pos              0                                          /*!< DWT CTRL: CYCCNTENA Position */
+#define DWT_CTRL_CYCCNTENA_Msk             (0x1UL << DWT_CTRL_CYCCNTENA_Pos)           /*!< DWT CTRL: CYCCNTENA Mask */
+
+/* DWT CPI Count Register Definitions */
+#define DWT_CPICNT_CPICNT_Pos               0                                          /*!< DWT CPICNT: CPICNT Position */
+#define DWT_CPICNT_CPICNT_Msk              (0xFFUL << DWT_CPICNT_CPICNT_Pos)           /*!< DWT CPICNT: CPICNT Mask */
+
+/* DWT Exception Overhead Count Register Definitions */
+#define DWT_EXCCNT_EXCCNT_Pos               0                                          /*!< DWT EXCCNT: EXCCNT Position */
+#define DWT_EXCCNT_EXCCNT_Msk              (0xFFUL << DWT_EXCCNT_EXCCNT_Pos)           /*!< DWT EXCCNT: EXCCNT Mask */
+
+/* DWT Sleep Count Register Definitions */
+#define DWT_SLEEPCNT_SLEEPCNT_Pos           0                                          /*!< DWT SLEEPCNT: SLEEPCNT Position */
+#define DWT_SLEEPCNT_SLEEPCNT_Msk          (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos)       /*!< DWT SLEEPCNT: SLEEPCNT Mask */
+
+/* DWT LSU Count Register Definitions */
+#define DWT_LSUCNT_LSUCNT_Pos               0                                          /*!< DWT LSUCNT: LSUCNT Position */
+#define DWT_LSUCNT_LSUCNT_Msk              (0xFFUL << DWT_LSUCNT_LSUCNT_Pos)           /*!< DWT LSUCNT: LSUCNT Mask */
+
+/* DWT Folded-instruction Count Register Definitions */
+#define DWT_FOLDCNT_FOLDCNT_Pos             0                                          /*!< DWT FOLDCNT: FOLDCNT Position */
+#define DWT_FOLDCNT_FOLDCNT_Msk            (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos)         /*!< DWT FOLDCNT: FOLDCNT Mask */
+
+/* DWT Comparator Mask Register Definitions */
+#define DWT_MASK_MASK_Pos                   0                                          /*!< DWT MASK: MASK Position */
+#define DWT_MASK_MASK_Msk                  (0x1FUL << DWT_MASK_MASK_Pos)               /*!< DWT MASK: MASK Mask */
+
+/* DWT Comparator Function Register Definitions */
+#define DWT_FUNCTION_MATCHED_Pos           24                                          /*!< DWT FUNCTION: MATCHED Position */
+#define DWT_FUNCTION_MATCHED_Msk           (0x1UL << DWT_FUNCTION_MATCHED_Pos)         /*!< DWT FUNCTION: MATCHED Mask */
+
+#define DWT_FUNCTION_DATAVADDR1_Pos        16                                          /*!< DWT FUNCTION: DATAVADDR1 Position */
+#define DWT_FUNCTION_DATAVADDR1_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos)      /*!< DWT FUNCTION: DATAVADDR1 Mask */
+
+#define DWT_FUNCTION_DATAVADDR0_Pos        12                                          /*!< DWT FUNCTION: DATAVADDR0 Position */
+#define DWT_FUNCTION_DATAVADDR0_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos)      /*!< DWT FUNCTION: DATAVADDR0 Mask */
+
+#define DWT_FUNCTION_DATAVSIZE_Pos         10                                          /*!< DWT FUNCTION: DATAVSIZE Position */
+#define DWT_FUNCTION_DATAVSIZE_Msk         (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos)       /*!< DWT FUNCTION: DATAVSIZE Mask */
+
+#define DWT_FUNCTION_LNK1ENA_Pos            9                                          /*!< DWT FUNCTION: LNK1ENA Position */
+#define DWT_FUNCTION_LNK1ENA_Msk           (0x1UL << DWT_FUNCTION_LNK1ENA_Pos)         /*!< DWT FUNCTION: LNK1ENA Mask */
+
+#define DWT_FUNCTION_DATAVMATCH_Pos         8                                          /*!< DWT FUNCTION: DATAVMATCH Position */
+#define DWT_FUNCTION_DATAVMATCH_Msk        (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos)      /*!< DWT FUNCTION: DATAVMATCH Mask */
+
+#define DWT_FUNCTION_CYCMATCH_Pos           7                                          /*!< DWT FUNCTION: CYCMATCH Position */
+#define DWT_FUNCTION_CYCMATCH_Msk          (0x1UL << DWT_FUNCTION_CYCMATCH_Pos)        /*!< DWT FUNCTION: CYCMATCH Mask */
+
+#define DWT_FUNCTION_EMITRANGE_Pos          5                                          /*!< DWT FUNCTION: EMITRANGE Position */
+#define DWT_FUNCTION_EMITRANGE_Msk         (0x1UL << DWT_FUNCTION_EMITRANGE_Pos)       /*!< DWT FUNCTION: EMITRANGE Mask */
+
+#define DWT_FUNCTION_FUNCTION_Pos           0                                          /*!< DWT FUNCTION: FUNCTION Position */
+#define DWT_FUNCTION_FUNCTION_Msk          (0xFUL << DWT_FUNCTION_FUNCTION_Pos)        /*!< DWT FUNCTION: FUNCTION Mask */
+
+/*@}*/ /* end of group CMSIS_DWT */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_TPI     Trace Port Interface (TPI)
+    \brief      Type definitions for the Trace Port Interface (TPI)
+  @{
+ */
+
+/** \brief  Structure type to access the Trace Port Interface Register (TPI).
+ */
+typedef struct
+{
+  __IO uint32_t SSPSR;                   /*!< Offset: 0x000 (R/ )  Supported Parallel Port Size Register     */
+  __IO uint32_t CSPSR;                   /*!< Offset: 0x004 (R/W)  Current Parallel Port Size Register */
+       uint32_t RESERVED0[2];
+  __IO uint32_t ACPR;                    /*!< Offset: 0x010 (R/W)  Asynchronous Clock Prescaler Register */
+       uint32_t RESERVED1[55];
+  __IO uint32_t SPPR;                    /*!< Offset: 0x0F0 (R/W)  Selected Pin Protocol Register */
+       uint32_t RESERVED2[131];
+  __I  uint32_t FFSR;                    /*!< Offset: 0x300 (R/ )  Formatter and Flush Status Register */
+  __IO uint32_t FFCR;                    /*!< Offset: 0x304 (R/W)  Formatter and Flush Control Register */
+  __I  uint32_t FSCR;                    /*!< Offset: 0x308 (R/ )  Formatter Synchronization Counter Register */
+       uint32_t RESERVED3[759];
+  __I  uint32_t TRIGGER;                 /*!< Offset: 0xEE8 (R/ )  TRIGGER */
+  __I  uint32_t FIFO0;                   /*!< Offset: 0xEEC (R/ )  Integration ETM Data */
+  __I  uint32_t ITATBCTR2;               /*!< Offset: 0xEF0 (R/ )  ITATBCTR2 */
+       uint32_t RESERVED4[1];
+  __I  uint32_t ITATBCTR0;               /*!< Offset: 0xEF8 (R/ )  ITATBCTR0 */
+  __I  uint32_t FIFO1;                   /*!< Offset: 0xEFC (R/ )  Integration ITM Data */
+  __IO uint32_t ITCTRL;                  /*!< Offset: 0xF00 (R/W)  Integration Mode Control */
+       uint32_t RESERVED5[39];
+  __IO uint32_t CLAIMSET;                /*!< Offset: 0xFA0 (R/W)  Claim tag set */
+  __IO uint32_t CLAIMCLR;                /*!< Offset: 0xFA4 (R/W)  Claim tag clear */
+       uint32_t RESERVED7[8];
+  __I  uint32_t DEVID;                   /*!< Offset: 0xFC8 (R/ )  TPIU_DEVID */
+  __I  uint32_t DEVTYPE;                 /*!< Offset: 0xFCC (R/ )  TPIU_DEVTYPE */
+} TPI_Type;
+
+/* TPI Asynchronous Clock Prescaler Register Definitions */
+#define TPI_ACPR_PRESCALER_Pos              0                                          /*!< TPI ACPR: PRESCALER Position */
+#define TPI_ACPR_PRESCALER_Msk             (0x1FFFUL << TPI_ACPR_PRESCALER_Pos)        /*!< TPI ACPR: PRESCALER Mask */
+
+/* TPI Selected Pin Protocol Register Definitions */
+#define TPI_SPPR_TXMODE_Pos                 0                                          /*!< TPI SPPR: TXMODE Position */
+#define TPI_SPPR_TXMODE_Msk                (0x3UL << TPI_SPPR_TXMODE_Pos)              /*!< TPI SPPR: TXMODE Mask */
+
+/* TPI Formatter and Flush Status Register Definitions */
+#define TPI_FFSR_FtNonStop_Pos              3                                          /*!< TPI FFSR: FtNonStop Position */
+#define TPI_FFSR_FtNonStop_Msk             (0x1UL << TPI_FFSR_FtNonStop_Pos)           /*!< TPI FFSR: FtNonStop Mask */
+
+#define TPI_FFSR_TCPresent_Pos              2                                          /*!< TPI FFSR: TCPresent Position */
+#define TPI_FFSR_TCPresent_Msk             (0x1UL << TPI_FFSR_TCPresent_Pos)           /*!< TPI FFSR: TCPresent Mask */
+
+#define TPI_FFSR_FtStopped_Pos              1                                          /*!< TPI FFSR: FtStopped Position */
+#define TPI_FFSR_FtStopped_Msk             (0x1UL << TPI_FFSR_FtStopped_Pos)           /*!< TPI FFSR: FtStopped Mask */
+
+#define TPI_FFSR_FlInProg_Pos               0                                          /*!< TPI FFSR: FlInProg Position */
+#define TPI_FFSR_FlInProg_Msk              (0x1UL << TPI_FFSR_FlInProg_Pos)            /*!< TPI FFSR: FlInProg Mask */
+
+/* TPI Formatter and Flush Control Register Definitions */
+#define TPI_FFCR_TrigIn_Pos                 8                                          /*!< TPI FFCR: TrigIn Position */
+#define TPI_FFCR_TrigIn_Msk                (0x1UL << TPI_FFCR_TrigIn_Pos)              /*!< TPI FFCR: TrigIn Mask */
+
+#define TPI_FFCR_EnFCont_Pos                1                                          /*!< TPI FFCR: EnFCont Position */
+#define TPI_FFCR_EnFCont_Msk               (0x1UL << TPI_FFCR_EnFCont_Pos)             /*!< TPI FFCR: EnFCont Mask */
+
+/* TPI TRIGGER Register Definitions */
+#define TPI_TRIGGER_TRIGGER_Pos             0                                          /*!< TPI TRIGGER: TRIGGER Position */
+#define TPI_TRIGGER_TRIGGER_Msk            (0x1UL << TPI_TRIGGER_TRIGGER_Pos)          /*!< TPI TRIGGER: TRIGGER Mask */
+
+/* TPI Integration ETM Data Register Definitions (FIFO0) */
+#define TPI_FIFO0_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO0: ITM_ATVALID Position */
+#define TPI_FIFO0_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos)        /*!< TPI FIFO0: ITM_ATVALID Mask */
+
+#define TPI_FIFO0_ITM_bytecount_Pos        27                                          /*!< TPI FIFO0: ITM_bytecount Position */
+#define TPI_FIFO0_ITM_bytecount_Msk        (0x3UL << TPI_FIFO0_ITM_bytecount_Pos)      /*!< TPI FIFO0: ITM_bytecount Mask */
+
+#define TPI_FIFO0_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO0: ETM_ATVALID Position */
+#define TPI_FIFO0_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos)        /*!< TPI FIFO0: ETM_ATVALID Mask */
+
+#define TPI_FIFO0_ETM_bytecount_Pos        24                                          /*!< TPI FIFO0: ETM_bytecount Position */
+#define TPI_FIFO0_ETM_bytecount_Msk        (0x3UL << TPI_FIFO0_ETM_bytecount_Pos)      /*!< TPI FIFO0: ETM_bytecount Mask */
+
+#define TPI_FIFO0_ETM2_Pos                 16                                          /*!< TPI FIFO0: ETM2 Position */
+#define TPI_FIFO0_ETM2_Msk                 (0xFFUL << TPI_FIFO0_ETM2_Pos)              /*!< TPI FIFO0: ETM2 Mask */
+
+#define TPI_FIFO0_ETM1_Pos                  8                                          /*!< TPI FIFO0: ETM1 Position */
+#define TPI_FIFO0_ETM1_Msk                 (0xFFUL << TPI_FIFO0_ETM1_Pos)              /*!< TPI FIFO0: ETM1 Mask */
+
+#define TPI_FIFO0_ETM0_Pos                  0                                          /*!< TPI FIFO0: ETM0 Position */
+#define TPI_FIFO0_ETM0_Msk                 (0xFFUL << TPI_FIFO0_ETM0_Pos)              /*!< TPI FIFO0: ETM0 Mask */
+
+/* TPI ITATBCTR2 Register Definitions */
+#define TPI_ITATBCTR2_ATREADY_Pos           0                                          /*!< TPI ITATBCTR2: ATREADY Position */
+#define TPI_ITATBCTR2_ATREADY_Msk          (0x1UL << TPI_ITATBCTR2_ATREADY_Pos)        /*!< TPI ITATBCTR2: ATREADY Mask */
+
+/* TPI Integration ITM Data Register Definitions (FIFO1) */
+#define TPI_FIFO1_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO1: ITM_ATVALID Position */
+#define TPI_FIFO1_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos)        /*!< TPI FIFO1: ITM_ATVALID Mask */
+
+#define TPI_FIFO1_ITM_bytecount_Pos        27                                          /*!< TPI FIFO1: ITM_bytecount Position */
+#define TPI_FIFO1_ITM_bytecount_Msk        (0x3UL << TPI_FIFO1_ITM_bytecount_Pos)      /*!< TPI FIFO1: ITM_bytecount Mask */
+
+#define TPI_FIFO1_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO1: ETM_ATVALID Position */
+#define TPI_FIFO1_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos)        /*!< TPI FIFO1: ETM_ATVALID Mask */
+
+#define TPI_FIFO1_ETM_bytecount_Pos        24                                          /*!< TPI FIFO1: ETM_bytecount Position */
+#define TPI_FIFO1_ETM_bytecount_Msk        (0x3UL << TPI_FIFO1_ETM_bytecount_Pos)      /*!< TPI FIFO1: ETM_bytecount Mask */
+
+#define TPI_FIFO1_ITM2_Pos                 16                                          /*!< TPI FIFO1: ITM2 Position */
+#define TPI_FIFO1_ITM2_Msk                 (0xFFUL << TPI_FIFO1_ITM2_Pos)              /*!< TPI FIFO1: ITM2 Mask */
+
+#define TPI_FIFO1_ITM1_Pos                  8                                          /*!< TPI FIFO1: ITM1 Position */
+#define TPI_FIFO1_ITM1_Msk                 (0xFFUL << TPI_FIFO1_ITM1_Pos)              /*!< TPI FIFO1: ITM1 Mask */
+
+#define TPI_FIFO1_ITM0_Pos                  0                                          /*!< TPI FIFO1: ITM0 Position */
+#define TPI_FIFO1_ITM0_Msk                 (0xFFUL << TPI_FIFO1_ITM0_Pos)              /*!< TPI FIFO1: ITM0 Mask */
+
+/* TPI ITATBCTR0 Register Definitions */
+#define TPI_ITATBCTR0_ATREADY_Pos           0                                          /*!< TPI ITATBCTR0: ATREADY Position */
+#define TPI_ITATBCTR0_ATREADY_Msk          (0x1UL << TPI_ITATBCTR0_ATREADY_Pos)        /*!< TPI ITATBCTR0: ATREADY Mask */
+
+/* TPI Integration Mode Control Register Definitions */
+#define TPI_ITCTRL_Mode_Pos                 0                                          /*!< TPI ITCTRL: Mode Position */
+#define TPI_ITCTRL_Mode_Msk                (0x1UL << TPI_ITCTRL_Mode_Pos)              /*!< TPI ITCTRL: Mode Mask */
+
+/* TPI DEVID Register Definitions */
+#define TPI_DEVID_NRZVALID_Pos             11                                          /*!< TPI DEVID: NRZVALID Position */
+#define TPI_DEVID_NRZVALID_Msk             (0x1UL << TPI_DEVID_NRZVALID_Pos)           /*!< TPI DEVID: NRZVALID Mask */
+
+#define TPI_DEVID_MANCVALID_Pos            10                                          /*!< TPI DEVID: MANCVALID Position */
+#define TPI_DEVID_MANCVALID_Msk            (0x1UL << TPI_DEVID_MANCVALID_Pos)          /*!< TPI DEVID: MANCVALID Mask */
+
+#define TPI_DEVID_PTINVALID_Pos             9                                          /*!< TPI DEVID: PTINVALID Position */
+#define TPI_DEVID_PTINVALID_Msk            (0x1UL << TPI_DEVID_PTINVALID_Pos)          /*!< TPI DEVID: PTINVALID Mask */
+
+#define TPI_DEVID_MinBufSz_Pos              6                                          /*!< TPI DEVID: MinBufSz Position */
+#define TPI_DEVID_MinBufSz_Msk             (0x7UL << TPI_DEVID_MinBufSz_Pos)           /*!< TPI DEVID: MinBufSz Mask */
+
+#define TPI_DEVID_AsynClkIn_Pos             5                                          /*!< TPI DEVID: AsynClkIn Position */
+#define TPI_DEVID_AsynClkIn_Msk            (0x1UL << TPI_DEVID_AsynClkIn_Pos)          /*!< TPI DEVID: AsynClkIn Mask */
+
+#define TPI_DEVID_NrTraceInput_Pos          0                                          /*!< TPI DEVID: NrTraceInput Position */
+#define TPI_DEVID_NrTraceInput_Msk         (0x1FUL << TPI_DEVID_NrTraceInput_Pos)      /*!< TPI DEVID: NrTraceInput Mask */
+
+/* TPI DEVTYPE Register Definitions */
+#define TPI_DEVTYPE_SubType_Pos             0                                          /*!< TPI DEVTYPE: SubType Position */
+#define TPI_DEVTYPE_SubType_Msk            (0xFUL << TPI_DEVTYPE_SubType_Pos)          /*!< TPI DEVTYPE: SubType Mask */
+
+#define TPI_DEVTYPE_MajorType_Pos           4                                          /*!< TPI DEVTYPE: MajorType Position */
+#define TPI_DEVTYPE_MajorType_Msk          (0xFUL << TPI_DEVTYPE_MajorType_Pos)        /*!< TPI DEVTYPE: MajorType Mask */
+
+/*@}*/ /* end of group CMSIS_TPI */
+
+
+#if (__MPU_PRESENT == 1)
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_MPU     Memory Protection Unit (MPU)
+    \brief      Type definitions for the Memory Protection Unit (MPU)
+  @{
+ */
+
+/** \brief  Structure type to access the Memory Protection Unit (MPU).
+ */
+typedef struct
+{
+  __I  uint32_t TYPE;                    /*!< Offset: 0x000 (R/ )  MPU Type Register                              */
+  __IO uint32_t CTRL;                    /*!< Offset: 0x004 (R/W)  MPU Control Register                           */
+  __IO uint32_t RNR;                     /*!< Offset: 0x008 (R/W)  MPU Region RNRber Register                     */
+  __IO uint32_t RBAR;                    /*!< Offset: 0x00C (R/W)  MPU Region Base Address Register               */
+  __IO uint32_t RASR;                    /*!< Offset: 0x010 (R/W)  MPU Region Attribute and Size Register         */
+  __IO uint32_t RBAR_A1;                 /*!< Offset: 0x014 (R/W)  MPU Alias 1 Region Base Address Register       */
+  __IO uint32_t RASR_A1;                 /*!< Offset: 0x018 (R/W)  MPU Alias 1 Region Attribute and Size Register */
+  __IO uint32_t RBAR_A2;                 /*!< Offset: 0x01C (R/W)  MPU Alias 2 Region Base Address Register       */
+  __IO uint32_t RASR_A2;                 /*!< Offset: 0x020 (R/W)  MPU Alias 2 Region Attribute and Size Register */
+  __IO uint32_t RBAR_A3;                 /*!< Offset: 0x024 (R/W)  MPU Alias 3 Region Base Address Register       */
+  __IO uint32_t RASR_A3;                 /*!< Offset: 0x028 (R/W)  MPU Alias 3 Region Attribute and Size Register */
+} MPU_Type;
+
+/* MPU Type Register */
+#define MPU_TYPE_IREGION_Pos               16                                             /*!< MPU TYPE: IREGION Position */
+#define MPU_TYPE_IREGION_Msk               (0xFFUL << MPU_TYPE_IREGION_Pos)               /*!< MPU TYPE: IREGION Mask */
+
+#define MPU_TYPE_DREGION_Pos                8                                             /*!< MPU TYPE: DREGION Position */
+#define MPU_TYPE_DREGION_Msk               (0xFFUL << MPU_TYPE_DREGION_Pos)               /*!< MPU TYPE: DREGION Mask */
+
+#define MPU_TYPE_SEPARATE_Pos               0                                             /*!< MPU TYPE: SEPARATE Position */
+#define MPU_TYPE_SEPARATE_Msk              (1UL << MPU_TYPE_SEPARATE_Pos)                 /*!< MPU TYPE: SEPARATE Mask */
+
+/* MPU Control Register */
+#define MPU_CTRL_PRIVDEFENA_Pos             2                                             /*!< MPU CTRL: PRIVDEFENA Position */
+#define MPU_CTRL_PRIVDEFENA_Msk            (1UL << MPU_CTRL_PRIVDEFENA_Pos)               /*!< MPU CTRL: PRIVDEFENA Mask */
+
+#define MPU_CTRL_HFNMIENA_Pos               1                                             /*!< MPU CTRL: HFNMIENA Position */
+#define MPU_CTRL_HFNMIENA_Msk              (1UL << MPU_CTRL_HFNMIENA_Pos)                 /*!< MPU CTRL: HFNMIENA Mask */
+
+#define MPU_CTRL_ENABLE_Pos                 0                                             /*!< MPU CTRL: ENABLE Position */
+#define MPU_CTRL_ENABLE_Msk                (1UL << MPU_CTRL_ENABLE_Pos)                   /*!< MPU CTRL: ENABLE Mask */
+
+/* MPU Region Number Register */
+#define MPU_RNR_REGION_Pos                  0                                             /*!< MPU RNR: REGION Position */
+#define MPU_RNR_REGION_Msk                 (0xFFUL << MPU_RNR_REGION_Pos)                 /*!< MPU RNR: REGION Mask */
+
+/* MPU Region Base Address Register */
+#define MPU_RBAR_ADDR_Pos                   5                                             /*!< MPU RBAR: ADDR Position */
+#define MPU_RBAR_ADDR_Msk                  (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos)             /*!< MPU RBAR: ADDR Mask */
+
+#define MPU_RBAR_VALID_Pos                  4                                             /*!< MPU RBAR: VALID Position */
+#define MPU_RBAR_VALID_Msk                 (1UL << MPU_RBAR_VALID_Pos)                    /*!< MPU RBAR: VALID Mask */
+
+#define MPU_RBAR_REGION_Pos                 0                                             /*!< MPU RBAR: REGION Position */
+#define MPU_RBAR_REGION_Msk                (0xFUL << MPU_RBAR_REGION_Pos)                 /*!< MPU RBAR: REGION Mask */
+
+/* MPU Region Attribute and Size Register */
+#define MPU_RASR_ATTRS_Pos                 16                                             /*!< MPU RASR: MPU Region Attribute field Position */
+#define MPU_RASR_ATTRS_Msk                 (0xFFFFUL << MPU_RASR_ATTRS_Pos)               /*!< MPU RASR: MPU Region Attribute field Mask */
+
+#define MPU_RASR_XN_Pos                    28                                             /*!< MPU RASR: ATTRS.XN Position */
+#define MPU_RASR_XN_Msk                    (1UL << MPU_RASR_XN_Pos)                       /*!< MPU RASR: ATTRS.XN Mask */
+
+#define MPU_RASR_AP_Pos                    24                                             /*!< MPU RASR: ATTRS.AP Position */
+#define MPU_RASR_AP_Msk                    (0x7UL << MPU_RASR_AP_Pos)                     /*!< MPU RASR: ATTRS.AP Mask */
+
+#define MPU_RASR_TEX_Pos                   19                                             /*!< MPU RASR: ATTRS.TEX Position */
+#define MPU_RASR_TEX_Msk                   (0x7UL << MPU_RASR_TEX_Pos)                    /*!< MPU RASR: ATTRS.TEX Mask */
+
+#define MPU_RASR_S_Pos                     18                                             /*!< MPU RASR: ATTRS.S Position */
+#define MPU_RASR_S_Msk                     (1UL << MPU_RASR_S_Pos)                        /*!< MPU RASR: ATTRS.S Mask */
+
+#define MPU_RASR_C_Pos                     17                                             /*!< MPU RASR: ATTRS.C Position */
+#define MPU_RASR_C_Msk                     (1UL << MPU_RASR_C_Pos)                        /*!< MPU RASR: ATTRS.C Mask */
+
+#define MPU_RASR_B_Pos                     16                                             /*!< MPU RASR: ATTRS.B Position */
+#define MPU_RASR_B_Msk                     (1UL << MPU_RASR_B_Pos)                        /*!< MPU RASR: ATTRS.B Mask */
+
+#define MPU_RASR_SRD_Pos                    8                                             /*!< MPU RASR: Sub-Region Disable Position */
+#define MPU_RASR_SRD_Msk                   (0xFFUL << MPU_RASR_SRD_Pos)                   /*!< MPU RASR: Sub-Region Disable Mask */
+
+#define MPU_RASR_SIZE_Pos                   1                                             /*!< MPU RASR: Region Size Field Position */
+#define MPU_RASR_SIZE_Msk                  (0x1FUL << MPU_RASR_SIZE_Pos)                  /*!< MPU RASR: Region Size Field Mask */
+
+#define MPU_RASR_ENABLE_Pos                 0                                             /*!< MPU RASR: Region enable bit Position */
+#define MPU_RASR_ENABLE_Msk                (1UL << MPU_RASR_ENABLE_Pos)                   /*!< MPU RASR: Region enable bit Disable Mask */
+
+/*@} end of group CMSIS_MPU */
+#endif
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_CoreDebug       Core Debug Registers (CoreDebug)
+    \brief      Type definitions for the Core Debug Registers
+  @{
+ */
+
+/** \brief  Structure type to access the Core Debug Register (CoreDebug).
+ */
+typedef struct
+{
+  __IO uint32_t DHCSR;                   /*!< Offset: 0x000 (R/W)  Debug Halting Control and Status Register    */
+  __O  uint32_t DCRSR;                   /*!< Offset: 0x004 ( /W)  Debug Core Register Selector Register        */
+  __IO uint32_t DCRDR;                   /*!< Offset: 0x008 (R/W)  Debug Core Register Data Register            */
+  __IO uint32_t DEMCR;                   /*!< Offset: 0x00C (R/W)  Debug Exception and Monitor Control Register */
+} CoreDebug_Type;
+
+/* Debug Halting Control and Status Register */
+#define CoreDebug_DHCSR_DBGKEY_Pos         16                                             /*!< CoreDebug DHCSR: DBGKEY Position */
+#define CoreDebug_DHCSR_DBGKEY_Msk         (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos)       /*!< CoreDebug DHCSR: DBGKEY Mask */
+
+#define CoreDebug_DHCSR_S_RESET_ST_Pos     25                                             /*!< CoreDebug DHCSR: S_RESET_ST Position */
+#define CoreDebug_DHCSR_S_RESET_ST_Msk     (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos)        /*!< CoreDebug DHCSR: S_RESET_ST Mask */
+
+#define CoreDebug_DHCSR_S_RETIRE_ST_Pos    24                                             /*!< CoreDebug DHCSR: S_RETIRE_ST Position */
+#define CoreDebug_DHCSR_S_RETIRE_ST_Msk    (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos)       /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */
+
+#define CoreDebug_DHCSR_S_LOCKUP_Pos       19                                             /*!< CoreDebug DHCSR: S_LOCKUP Position */
+#define CoreDebug_DHCSR_S_LOCKUP_Msk       (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos)          /*!< CoreDebug DHCSR: S_LOCKUP Mask */
+
+#define CoreDebug_DHCSR_S_SLEEP_Pos        18                                             /*!< CoreDebug DHCSR: S_SLEEP Position */
+#define CoreDebug_DHCSR_S_SLEEP_Msk        (1UL << CoreDebug_DHCSR_S_SLEEP_Pos)           /*!< CoreDebug DHCSR: S_SLEEP Mask */
+
+#define CoreDebug_DHCSR_S_HALT_Pos         17                                             /*!< CoreDebug DHCSR: S_HALT Position */
+#define CoreDebug_DHCSR_S_HALT_Msk         (1UL << CoreDebug_DHCSR_S_HALT_Pos)            /*!< CoreDebug DHCSR: S_HALT Mask */
+
+#define CoreDebug_DHCSR_S_REGRDY_Pos       16                                             /*!< CoreDebug DHCSR: S_REGRDY Position */
+#define CoreDebug_DHCSR_S_REGRDY_Msk       (1UL << CoreDebug_DHCSR_S_REGRDY_Pos)          /*!< CoreDebug DHCSR: S_REGRDY Mask */
+
+#define CoreDebug_DHCSR_C_SNAPSTALL_Pos     5                                             /*!< CoreDebug DHCSR: C_SNAPSTALL Position */
+#define CoreDebug_DHCSR_C_SNAPSTALL_Msk    (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos)       /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */
+
+#define CoreDebug_DHCSR_C_MASKINTS_Pos      3                                             /*!< CoreDebug DHCSR: C_MASKINTS Position */
+#define CoreDebug_DHCSR_C_MASKINTS_Msk     (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos)        /*!< CoreDebug DHCSR: C_MASKINTS Mask */
+
+#define CoreDebug_DHCSR_C_STEP_Pos          2                                             /*!< CoreDebug DHCSR: C_STEP Position */
+#define CoreDebug_DHCSR_C_STEP_Msk         (1UL << CoreDebug_DHCSR_C_STEP_Pos)            /*!< CoreDebug DHCSR: C_STEP Mask */
+
+#define CoreDebug_DHCSR_C_HALT_Pos          1                                             /*!< CoreDebug DHCSR: C_HALT Position */
+#define CoreDebug_DHCSR_C_HALT_Msk         (1UL << CoreDebug_DHCSR_C_HALT_Pos)            /*!< CoreDebug DHCSR: C_HALT Mask */
+
+#define CoreDebug_DHCSR_C_DEBUGEN_Pos       0                                             /*!< CoreDebug DHCSR: C_DEBUGEN Position */
+#define CoreDebug_DHCSR_C_DEBUGEN_Msk      (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos)         /*!< CoreDebug DHCSR: C_DEBUGEN Mask */
+
+/* Debug Core Register Selector Register */
+#define CoreDebug_DCRSR_REGWnR_Pos         16                                             /*!< CoreDebug DCRSR: REGWnR Position */
+#define CoreDebug_DCRSR_REGWnR_Msk         (1UL << CoreDebug_DCRSR_REGWnR_Pos)            /*!< CoreDebug DCRSR: REGWnR Mask */
+
+#define CoreDebug_DCRSR_REGSEL_Pos          0                                             /*!< CoreDebug DCRSR: REGSEL Position */
+#define CoreDebug_DCRSR_REGSEL_Msk         (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos)         /*!< CoreDebug DCRSR: REGSEL Mask */
+
+/* Debug Exception and Monitor Control Register */
+#define CoreDebug_DEMCR_TRCENA_Pos         24                                             /*!< CoreDebug DEMCR: TRCENA Position */
+#define CoreDebug_DEMCR_TRCENA_Msk         (1UL << CoreDebug_DEMCR_TRCENA_Pos)            /*!< CoreDebug DEMCR: TRCENA Mask */
+
+#define CoreDebug_DEMCR_MON_REQ_Pos        19                                             /*!< CoreDebug DEMCR: MON_REQ Position */
+#define CoreDebug_DEMCR_MON_REQ_Msk        (1UL << CoreDebug_DEMCR_MON_REQ_Pos)           /*!< CoreDebug DEMCR: MON_REQ Mask */
+
+#define CoreDebug_DEMCR_MON_STEP_Pos       18                                             /*!< CoreDebug DEMCR: MON_STEP Position */
+#define CoreDebug_DEMCR_MON_STEP_Msk       (1UL << CoreDebug_DEMCR_MON_STEP_Pos)          /*!< CoreDebug DEMCR: MON_STEP Mask */
+
+#define CoreDebug_DEMCR_MON_PEND_Pos       17                                             /*!< CoreDebug DEMCR: MON_PEND Position */
+#define CoreDebug_DEMCR_MON_PEND_Msk       (1UL << CoreDebug_DEMCR_MON_PEND_Pos)          /*!< CoreDebug DEMCR: MON_PEND Mask */
+
+#define CoreDebug_DEMCR_MON_EN_Pos         16                                             /*!< CoreDebug DEMCR: MON_EN Position */
+#define CoreDebug_DEMCR_MON_EN_Msk         (1UL << CoreDebug_DEMCR_MON_EN_Pos)            /*!< CoreDebug DEMCR: MON_EN Mask */
+
+#define CoreDebug_DEMCR_VC_HARDERR_Pos     10                                             /*!< CoreDebug DEMCR: VC_HARDERR Position */
+#define CoreDebug_DEMCR_VC_HARDERR_Msk     (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos)        /*!< CoreDebug DEMCR: VC_HARDERR Mask */
+
+#define CoreDebug_DEMCR_VC_INTERR_Pos       9                                             /*!< CoreDebug DEMCR: VC_INTERR Position */
+#define CoreDebug_DEMCR_VC_INTERR_Msk      (1UL << CoreDebug_DEMCR_VC_INTERR_Pos)         /*!< CoreDebug DEMCR: VC_INTERR Mask */
+
+#define CoreDebug_DEMCR_VC_BUSERR_Pos       8                                             /*!< CoreDebug DEMCR: VC_BUSERR Position */
+#define CoreDebug_DEMCR_VC_BUSERR_Msk      (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos)         /*!< CoreDebug DEMCR: VC_BUSERR Mask */
+
+#define CoreDebug_DEMCR_VC_STATERR_Pos      7                                             /*!< CoreDebug DEMCR: VC_STATERR Position */
+#define CoreDebug_DEMCR_VC_STATERR_Msk     (1UL << CoreDebug_DEMCR_VC_STATERR_Pos)        /*!< CoreDebug DEMCR: VC_STATERR Mask */
+
+#define CoreDebug_DEMCR_VC_CHKERR_Pos       6                                             /*!< CoreDebug DEMCR: VC_CHKERR Position */
+#define CoreDebug_DEMCR_VC_CHKERR_Msk      (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos)         /*!< CoreDebug DEMCR: VC_CHKERR Mask */
+
+#define CoreDebug_DEMCR_VC_NOCPERR_Pos      5                                             /*!< CoreDebug DEMCR: VC_NOCPERR Position */
+#define CoreDebug_DEMCR_VC_NOCPERR_Msk     (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos)        /*!< CoreDebug DEMCR: VC_NOCPERR Mask */
+
+#define CoreDebug_DEMCR_VC_MMERR_Pos        4                                             /*!< CoreDebug DEMCR: VC_MMERR Position */
+#define CoreDebug_DEMCR_VC_MMERR_Msk       (1UL << CoreDebug_DEMCR_VC_MMERR_Pos)          /*!< CoreDebug DEMCR: VC_MMERR Mask */
+
+#define CoreDebug_DEMCR_VC_CORERESET_Pos    0                                             /*!< CoreDebug DEMCR: VC_CORERESET Position */
+#define CoreDebug_DEMCR_VC_CORERESET_Msk   (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos)      /*!< CoreDebug DEMCR: VC_CORERESET Mask */
+
+/*@} end of group CMSIS_CoreDebug */
+
+
+/** \ingroup    CMSIS_core_register
+    \defgroup   CMSIS_core_base     Core Definitions
+    \brief      Definitions for base addresses, unions, and structures.
+  @{
+ */
+
+/* Memory mapping of Cortex-M3 Hardware */
+#define SCS_BASE            (0xE000E000UL)                            /*!< System Control Space Base Address  */
+#define ITM_BASE            (0xE0000000UL)                            /*!< ITM Base Address                   */
+#define DWT_BASE            (0xE0001000UL)                            /*!< DWT Base Address                   */
+#define TPI_BASE            (0xE0040000UL)                            /*!< TPI Base Address                   */
+#define CoreDebug_BASE      (0xE000EDF0UL)                            /*!< Core Debug Base Address            */
+#define SysTick_BASE        (SCS_BASE +  0x0010UL)                    /*!< SysTick Base Address               */
+#define NVIC_BASE           (SCS_BASE +  0x0100UL)                    /*!< NVIC Base Address                  */
+#define SCB_BASE            (SCS_BASE +  0x0D00UL)                    /*!< System Control Block Base Address  */
+
+#define SCnSCB              ((SCnSCB_Type    *)     SCS_BASE      )   /*!< System control Register not in SCB */
+#define SCB                 ((SCB_Type       *)     SCB_BASE      )   /*!< SCB configuration struct           */
+#define SysTick             ((SysTick_Type   *)     SysTick_BASE  )   /*!< SysTick configuration struct       */
+#define NVIC                ((NVIC_Type      *)     NVIC_BASE     )   /*!< NVIC configuration struct          */
+#define ITM                 ((ITM_Type       *)     ITM_BASE      )   /*!< ITM configuration struct           */
+#define DWT                 ((DWT_Type       *)     DWT_BASE      )   /*!< DWT configuration struct           */
+#define TPI                 ((TPI_Type       *)     TPI_BASE      )   /*!< TPI configuration struct           */
+#define CoreDebug           ((CoreDebug_Type *)     CoreDebug_BASE)   /*!< Core Debug configuration struct    */
+
+#if (__MPU_PRESENT == 1)
+  #define MPU_BASE          (SCS_BASE +  0x0D90UL)                    /*!< Memory Protection Unit             */
+  #define MPU               ((MPU_Type       *)     MPU_BASE      )   /*!< Memory Protection Unit             */
+#endif
+
+/*@} */
+
+
+
+/*******************************************************************************
+ *                Hardware Abstraction Layer
+  Core Function Interface contains:
+  - Core NVIC Functions
+  - Core SysTick Functions
+  - Core Debug Functions
+  - Core Register Access Functions
+ ******************************************************************************/
+/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
+*/
+
+
+
+/* ##########################   NVIC functions  #################################### */
+/** \ingroup  CMSIS_Core_FunctionInterface
+    \defgroup CMSIS_Core_NVICFunctions NVIC Functions
+    \brief      Functions that manage interrupts and exceptions via the NVIC.
+    @{
+ */
+
+/** \brief  Set Priority Grouping
+
+  The function sets the priority grouping field using the required unlock sequence.
+  The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.
+  Only values from 0..7 are used.
+  In case of a conflict between priority grouping and available
+  priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
+
+    \param [in]      PriorityGroup  Priority grouping field.
+ */
+__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
+{
+  uint32_t reg_value;
+  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07);               /* only values 0..7 are used          */
+
+  reg_value  =  SCB->AIRCR;                                                   /* read old register configuration    */
+  reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk);             /* clear bits to change               */
+  reg_value  =  (reg_value                                 |
+                ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |
+                (PriorityGroupTmp << 8));                                     /* Insert write key and priorty group */
+  SCB->AIRCR =  reg_value;
+}
+
+
+/** \brief  Get Priority Grouping
+
+  The function reads the priority grouping field from the NVIC Interrupt Controller.
+
+    \return                Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).
+ */
+__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
+{
+  return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos);   /* read priority grouping field */
+}
+
+
+/** \brief  Enable External Interrupt
+
+    The function enables a device-specific interrupt in the NVIC interrupt controller.
+
+    \param [in]      IRQn  External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
+{
+  NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* enable interrupt */
+}
+
+
+/** \brief  Disable External Interrupt
+
+    The function disables a device-specific interrupt in the NVIC interrupt controller.
+
+    \param [in]      IRQn  External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
+{
+  NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */
+}
+
+
+/** \brief  Get Pending Interrupt
+
+    The function reads the pending register in the NVIC and returns the pending bit
+    for the specified interrupt.
+
+    \param [in]      IRQn  Interrupt number.
+
+    \return             0  Interrupt status is not pending.
+    \return             1  Interrupt status is pending.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
+{
+  return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */
+}
+
+
+/** \brief  Set Pending Interrupt
+
+    The function sets the pending bit of an external interrupt.
+
+    \param [in]      IRQn  Interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
+{
+  NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */
+}
+
+
+/** \brief  Clear Pending Interrupt
+
+    The function clears the pending bit of an external interrupt.
+
+    \param [in]      IRQn  External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
+{
+  NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
+}
+
+
+/** \brief  Get Active Interrupt
+
+    The function reads the active register in NVIC and returns the active bit.
+
+    \param [in]      IRQn  Interrupt number.
+
+    \return             0  Interrupt status is not active.
+    \return             1  Interrupt status is active.
+ */
+__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
+{
+  return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */
+}
+
+
+/** \brief  Set Interrupt Priority
+
+    The function sets the priority of an interrupt.
+
+    \note The priority cannot be set for every core interrupt.
+
+    \param [in]      IRQn  Interrupt number.
+    \param [in]  priority  Priority to set.
+ */
+__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
+{
+  if(IRQn < 0) {
+    SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M  System Interrupts */
+  else {
+    NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff);    }        /* set Priority for device specific Interrupts  */
+}
+
+
+/** \brief  Get Interrupt Priority
+
+    The function reads the priority of an interrupt. The interrupt
+    number can be positive to specify an external (device specific)
+    interrupt, or negative to specify an internal (core) interrupt.
+
+
+    \param [in]   IRQn  Interrupt number.
+    \return             Interrupt Priority. Value is aligned automatically to the implemented
+                        priority bits of the microcontroller.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
+{
+
+  if(IRQn < 0) {
+    return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for Cortex-M  system interrupts */
+  else {
+    return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)]           >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for device specific interrupts  */
+}
+
+
+/** \brief  Encode Priority
+
+    The function encodes the priority for an interrupt with the given priority group,
+    preemptive priority value, and subpriority value.
+    In case of a conflict between priority grouping and available
+    priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.
+
+    \param [in]     PriorityGroup  Used priority group.
+    \param [in]   PreemptPriority  Preemptive priority value (starting from 0).
+    \param [in]       SubPriority  Subpriority value (starting from 0).
+    \return                        Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
+ */
+__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
+{
+  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */
+  uint32_t PreemptPriorityBits;
+  uint32_t SubPriorityBits;
+
+  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
+  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
+
+  return (
+           ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |
+           ((SubPriority     & ((1 << (SubPriorityBits    )) - 1)))
+         );
+}
+
+
+/** \brief  Decode Priority
+
+    The function decodes an interrupt priority value with a given priority group to
+    preemptive priority value and subpriority value.
+    In case of a conflict between priority grouping and available
+    priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.
+
+    \param [in]         Priority   Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
+    \param [in]     PriorityGroup  Used priority group.
+    \param [out] pPreemptPriority  Preemptive priority value (starting from 0).
+    \param [out]     pSubPriority  Subpriority value (starting from 0).
+ */
+__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)
+{
+  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */
+  uint32_t PreemptPriorityBits;
+  uint32_t SubPriorityBits;
+
+  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
+  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
+
+  *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);
+  *pSubPriority     = (Priority                   ) & ((1 << (SubPriorityBits    )) - 1);
+}
+
+
+/** \brief  System Reset
+
+    The function initiates a system reset request to reset the MCU.
+ */
+__STATIC_INLINE void NVIC_SystemReset(void)
+{
+  __DSB();                                                     /* Ensure all outstanding memory accesses included
+                                                                  buffered write are completed before reset */
+  SCB->AIRCR  = ((0x5FA << SCB_AIRCR_VECTKEY_Pos)      |
+                 (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
+                 SCB_AIRCR_SYSRESETREQ_Msk);                   /* Keep priority group unchanged */
+  __DSB();                                                     /* Ensure completion of memory access */
+  while(1);                                                    /* wait until reset */
+}
+
+/*@} end of CMSIS_Core_NVICFunctions */
+
+
+
+/* ##################################    SysTick function  ############################################ */
+/** \ingroup  CMSIS_Core_FunctionInterface
+    \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
+    \brief      Functions that configure the System.
+  @{
+ */
+
+#if (__Vendor_SysTickConfig == 0)
+
+/** \brief  System Tick Configuration
+
+    The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
+    Counter is in free running mode to generate periodic interrupts.
+
+    \param [in]  ticks  Number of ticks between two interrupts.
+
+    \return          0  Function succeeded.
+    \return          1  Function failed.
+
+    \note     When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
+    function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
+    must contain a vendor-specific implementation of this function.
+
+ */
+__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
+{
+  if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk)  return (1);      /* Reload value impossible */
+
+  SysTick->LOAD  = ticks - 1;                                  /* set reload register */
+  NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1);  /* set Priority for Systick Interrupt */
+  SysTick->VAL   = 0;                                          /* Load the SysTick Counter Value */
+  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |
+                   SysTick_CTRL_TICKINT_Msk   |
+                   SysTick_CTRL_ENABLE_Msk;                    /* Enable SysTick IRQ and SysTick Timer */
+  return (0);                                                  /* Function successful */
+}
+
+#endif
+
+/*@} end of CMSIS_Core_SysTickFunctions */
+
+
+
+/* ##################################### Debug In/Output function ########################################### */
+/** \ingroup  CMSIS_Core_FunctionInterface
+    \defgroup CMSIS_core_DebugFunctions ITM Functions
+    \brief   Functions that access the ITM debug interface.
+  @{
+ */
+
+extern volatile int32_t ITM_RxBuffer;                    /*!< External variable to receive characters.                         */
+#define                 ITM_RXBUFFER_EMPTY    0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */
+
+
+/** \brief  ITM Send Character
+
+    The function transmits a character via the ITM channel 0, and
+    \li Just returns when no debugger is connected that has booked the output.
+    \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.
+
+    \param [in]     ch  Character to transmit.
+
+    \returns            Character to transmit.
+ */
+__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
+{
+  if ((ITM->TCR & ITM_TCR_ITMENA_Msk)                  &&      /* ITM enabled */
+      (ITM->TER & (1UL << 0)        )                    )     /* ITM Port #0 enabled */
+  {
+    while (ITM->PORT[0].u32 == 0);
+    ITM->PORT[0].u8 = (uint8_t) ch;
+  }
+  return (ch);
+}
+
+
+/** \brief  ITM Receive Character
+
+    The function inputs a character via the external variable \ref ITM_RxBuffer.
+
+    \return             Received character.
+    \return         -1  No character pending.
+ */
+__STATIC_INLINE int32_t ITM_ReceiveChar (void) {
+  int32_t ch = -1;                           /* no character available */
+
+  if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {
+    ch = ITM_RxBuffer;
+    ITM_RxBuffer = ITM_RXBUFFER_EMPTY;       /* ready for next character */
+  }
+
+  return (ch);
+}
+
+
+/** \brief  ITM Check Character
+
+    The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.
+
+    \return          0  No character available.
+    \return          1  Character available.
+ */
+__STATIC_INLINE int32_t ITM_CheckChar (void) {
+
+  if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {
+    return (0);                                 /* no character available */
+  } else {
+    return (1);                                 /*    character available */
+  }
+}
+
+/*@} end of CMSIS_core_DebugFunctions */
+
+#endif /* __CORE_CM3_H_DEPENDANT */
+
+#endif /* __CMSIS_GENERIC */
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/lib/mathlib/CMSIS/Include/core_cm4.h b/src/lib/mathlib/CMSIS/Include/core_cm4.h
new file mode 100644
index 000000000..93efd3a7a
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Include/core_cm4.h
@@ -0,0 +1,1772 @@
+/**************************************************************************//**
+ * @file     core_cm4.h
+ * @brief    CMSIS Cortex-M4 Core Peripheral Access Layer Header File
+ * @version  V3.20
+ * @date     25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+   All rights reserved.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+   - Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+   ---------------------------------------------------------------------------*/
+
+
+#if defined ( __ICCARM__ )
+ #pragma system_include  /* treat file as system include file for MISRA check */
+#endif
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#ifndef __CORE_CM4_H_GENERIC
+#define __CORE_CM4_H_GENERIC
+
+/** \page CMSIS_MISRA_Exceptions  MISRA-C:2004 Compliance Exceptions
+  CMSIS violates the following MISRA-C:2004 rules:
+
+   \li Required Rule 8.5, object/function definition in header file.<br>
+     Function definitions in header files are used to allow 'inlining'.
+
+   \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
+     Unions are used for effective representation of core registers.
+
+   \li Advisory Rule 19.7, Function-like macro defined.<br>
+     Function-like macros are used to allow more efficient code.
+ */
+
+
+/*******************************************************************************
+ *                 CMSIS definitions
+ ******************************************************************************/
+/** \ingroup Cortex_M4
+  @{
+ */
+
+/*  CMSIS CM4 definitions */
+#define __CM4_CMSIS_VERSION_MAIN  (0x03)                                   /*!< [31:16] CMSIS HAL main version   */
+#define __CM4_CMSIS_VERSION_SUB   (0x20)                                   /*!< [15:0]  CMSIS HAL sub version    */
+#define __CM4_CMSIS_VERSION       ((__CM4_CMSIS_VERSION_MAIN << 16) | \
+                                    __CM4_CMSIS_VERSION_SUB          )     /*!< CMSIS HAL version number         */
+
+#define __CORTEX_M                (0x04)                                   /*!< Cortex-M Core                    */
+
+
+#if   defined ( __CC_ARM )
+  #define __ASM            __asm                                      /*!< asm keyword for ARM Compiler          */
+  #define __INLINE         __inline                                   /*!< inline keyword for ARM Compiler       */
+  #define __STATIC_INLINE  static __inline
+
+#elif defined ( __ICCARM__ )
+  #define __ASM            __asm                                      /*!< asm keyword for IAR Compiler          */
+  #define __INLINE         inline                                     /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */
+  #define __STATIC_INLINE  static inline
+
+#elif defined ( __TMS470__ )
+  #define __ASM            __asm                                      /*!< asm keyword for TI CCS Compiler       */
+  #define __STATIC_INLINE  static inline
+
+#elif defined ( __GNUC__ )
+  #define __ASM            __asm                                      /*!< asm keyword for GNU Compiler          */
+  #define __INLINE         inline                                     /*!< inline keyword for GNU Compiler       */
+  #define __STATIC_INLINE  static inline
+
+#elif defined ( __TASKING__ )
+  #define __ASM            __asm                                      /*!< asm keyword for TASKING Compiler      */
+  #define __INLINE         inline                                     /*!< inline keyword for TASKING Compiler   */
+  #define __STATIC_INLINE  static inline
+
+#endif
+
+/** __FPU_USED indicates whether an FPU is used or not. For this, __FPU_PRESENT has to be checked prior to making use of FPU specific registers and functions.
+*/
+#if defined ( __CC_ARM )
+  #if defined __TARGET_FPU_VFP
+    #if (__FPU_PRESENT == 1)
+      #define __FPU_USED       1
+    #else
+      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+      #define __FPU_USED       0
+    #endif
+  #else
+    #define __FPU_USED         0
+  #endif
+
+#elif defined ( __ICCARM__ )
+  #if defined __ARMVFP__
+    #if (__FPU_PRESENT == 1)
+      #define __FPU_USED       1
+    #else
+      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+      #define __FPU_USED       0
+    #endif
+  #else
+    #define __FPU_USED         0
+  #endif
+
+#elif defined ( __TMS470__ )
+  #if defined __TI_VFP_SUPPORT__
+    #if (__FPU_PRESENT == 1)
+      #define __FPU_USED       1
+    #else
+      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+      #define __FPU_USED       0
+    #endif
+  #else
+    #define __FPU_USED         0
+  #endif
+
+#elif defined ( __GNUC__ )
+  #if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    #if (__FPU_PRESENT == 1)
+      #define __FPU_USED       1
+    #else
+      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+      #define __FPU_USED       0
+    #endif
+  #else
+    #define __FPU_USED         0
+  #endif
+
+#elif defined ( __TASKING__ )
+  #if defined __FPU_VFP__
+    #if (__FPU_PRESENT == 1)
+      #define __FPU_USED       1
+    #else
+      #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+      #define __FPU_USED       0
+    #endif
+  #else
+    #define __FPU_USED         0
+  #endif
+#endif
+
+#include <stdint.h>                      /* standard types definitions                      */
+#include "core_cmInstr.h"                /* Core Instruction Access                         */
+#include "core_cmFunc.h"                 /* Core Function Access                            */
+#include "core_cm4_simd.h"               /* Compiler specific SIMD Intrinsics               */
+
+#endif /* __CORE_CM4_H_GENERIC */
+
+#ifndef __CMSIS_GENERIC
+
+#ifndef __CORE_CM4_H_DEPENDANT
+#define __CORE_CM4_H_DEPENDANT
+
+/* check device defines and use defaults */
+#if defined __CHECK_DEVICE_DEFINES
+  #ifndef __CM4_REV
+    #define __CM4_REV               0x0000
+    #warning "__CM4_REV not defined in device header file; using default!"
+  #endif
+
+  #ifndef __FPU_PRESENT
+    #define __FPU_PRESENT             0
+    #warning "__FPU_PRESENT not defined in device header file; using default!"
+  #endif
+
+  #ifndef __MPU_PRESENT
+    #define __MPU_PRESENT             0
+    #warning "__MPU_PRESENT not defined in device header file; using default!"
+  #endif
+
+  #ifndef __NVIC_PRIO_BITS
+    #define __NVIC_PRIO_BITS          4
+    #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
+  #endif
+
+  #ifndef __Vendor_SysTickConfig
+    #define __Vendor_SysTickConfig    0
+    #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
+  #endif
+#endif
+
+/* IO definitions (access restrictions to peripheral registers) */
+/**
+    \defgroup CMSIS_glob_defs CMSIS Global Defines
+
+    <strong>IO Type Qualifiers</strong> are used
+    \li to specify the access to peripheral variables.
+    \li for automatic generation of peripheral register debug information.
+*/
+#ifdef __cplusplus
+  #define   __I     volatile             /*!< Defines 'read only' permissions                 */
+#else
+  #define   __I     volatile const       /*!< Defines 'read only' permissions                 */
+#endif
+#define     __O     volatile             /*!< Defines 'write only' permissions                */
+#define     __IO    volatile             /*!< Defines 'read / write' permissions              */
+
+/*@} end of group Cortex_M4 */
+
+
+
+/*******************************************************************************
+ *                 Register Abstraction
+  Core Register contain:
+  - Core Register
+  - Core NVIC Register
+  - Core SCB Register
+  - Core SysTick Register
+  - Core Debug Register
+  - Core MPU Register
+  - Core FPU Register
+ ******************************************************************************/
+/** \defgroup CMSIS_core_register Defines and Type Definitions
+    \brief Type definitions and defines for Cortex-M processor based devices.
+*/
+
+/** \ingroup    CMSIS_core_register
+    \defgroup   CMSIS_CORE  Status and Control Registers
+    \brief  Core Register type definitions.
+  @{
+ */
+
+/** \brief  Union type to access the Application Program Status Register (APSR).
+ */
+typedef union
+{
+  struct
+  {
+#if (__CORTEX_M != 0x04)
+    uint32_t _reserved0:27;              /*!< bit:  0..26  Reserved                           */
+#else
+    uint32_t _reserved0:16;              /*!< bit:  0..15  Reserved                           */
+    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */
+    uint32_t _reserved1:7;               /*!< bit: 20..26  Reserved                           */
+#endif
+    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */
+    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */
+    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */
+    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */
+    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */
+  } b;                                   /*!< Structure used for bit  access                  */
+  uint32_t w;                            /*!< Type      used for word access                  */
+} APSR_Type;
+
+
+/** \brief  Union type to access the Interrupt Program Status Register (IPSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */
+    uint32_t _reserved0:23;              /*!< bit:  9..31  Reserved                           */
+  } b;                                   /*!< Structure used for bit  access                  */
+  uint32_t w;                            /*!< Type      used for word access                  */
+} IPSR_Type;
+
+
+/** \brief  Union type to access the Special-Purpose Program Status Registers (xPSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */
+#if (__CORTEX_M != 0x04)
+    uint32_t _reserved0:15;              /*!< bit:  9..23  Reserved                           */
+#else
+    uint32_t _reserved0:7;               /*!< bit:  9..15  Reserved                           */
+    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */
+    uint32_t _reserved1:4;               /*!< bit: 20..23  Reserved                           */
+#endif
+    uint32_t T:1;                        /*!< bit:     24  Thumb bit        (read 0)          */
+    uint32_t IT:2;                       /*!< bit: 25..26  saved IT state   (read 0)          */
+    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */
+    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */
+    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */
+    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */
+    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */
+  } b;                                   /*!< Structure used for bit  access                  */
+  uint32_t w;                            /*!< Type      used for word access                  */
+} xPSR_Type;
+
+
+/** \brief  Union type to access the Control Registers (CONTROL).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t nPRIV:1;                    /*!< bit:      0  Execution privilege in Thread mode */
+    uint32_t SPSEL:1;                    /*!< bit:      1  Stack to be used                   */
+    uint32_t FPCA:1;                     /*!< bit:      2  FP extension active flag           */
+    uint32_t _reserved0:29;              /*!< bit:  3..31  Reserved                           */
+  } b;                                   /*!< Structure used for bit  access                  */
+  uint32_t w;                            /*!< Type      used for word access                  */
+} CONTROL_Type;
+
+/*@} end of group CMSIS_CORE */
+
+
+/** \ingroup    CMSIS_core_register
+    \defgroup   CMSIS_NVIC  Nested Vectored Interrupt Controller (NVIC)
+    \brief      Type definitions for the NVIC Registers
+  @{
+ */
+
+/** \brief  Structure type to access the Nested Vectored Interrupt Controller (NVIC).
+ */
+typedef struct
+{
+  __IO uint32_t ISER[8];                 /*!< Offset: 0x000 (R/W)  Interrupt Set Enable Register           */
+       uint32_t RESERVED0[24];
+  __IO uint32_t ICER[8];                 /*!< Offset: 0x080 (R/W)  Interrupt Clear Enable Register         */
+       uint32_t RSERVED1[24];
+  __IO uint32_t ISPR[8];                 /*!< Offset: 0x100 (R/W)  Interrupt Set Pending Register          */
+       uint32_t RESERVED2[24];
+  __IO uint32_t ICPR[8];                 /*!< Offset: 0x180 (R/W)  Interrupt Clear Pending Register        */
+       uint32_t RESERVED3[24];
+  __IO uint32_t IABR[8];                 /*!< Offset: 0x200 (R/W)  Interrupt Active bit Register           */
+       uint32_t RESERVED4[56];
+  __IO uint8_t  IP[240];                 /*!< Offset: 0x300 (R/W)  Interrupt Priority Register (8Bit wide) */
+       uint32_t RESERVED5[644];
+  __O  uint32_t STIR;                    /*!< Offset: 0xE00 ( /W)  Software Trigger Interrupt Register     */
+}  NVIC_Type;
+
+/* Software Triggered Interrupt Register Definitions */
+#define NVIC_STIR_INTID_Pos                 0                                          /*!< STIR: INTLINESNUM Position */
+#define NVIC_STIR_INTID_Msk                (0x1FFUL << NVIC_STIR_INTID_Pos)            /*!< STIR: INTLINESNUM Mask */
+
+/*@} end of group CMSIS_NVIC */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_SCB     System Control Block (SCB)
+    \brief      Type definitions for the System Control Block Registers
+  @{
+ */
+
+/** \brief  Structure type to access the System Control Block (SCB).
+ */
+typedef struct
+{
+  __I  uint32_t CPUID;                   /*!< Offset: 0x000 (R/ )  CPUID Base Register                                   */
+  __IO uint32_t ICSR;                    /*!< Offset: 0x004 (R/W)  Interrupt Control and State Register                  */
+  __IO uint32_t VTOR;                    /*!< Offset: 0x008 (R/W)  Vector Table Offset Register                          */
+  __IO uint32_t AIRCR;                   /*!< Offset: 0x00C (R/W)  Application Interrupt and Reset Control Register      */
+  __IO uint32_t SCR;                     /*!< Offset: 0x010 (R/W)  System Control Register                               */
+  __IO uint32_t CCR;                     /*!< Offset: 0x014 (R/W)  Configuration Control Register                        */
+  __IO uint8_t  SHP[12];                 /*!< Offset: 0x018 (R/W)  System Handlers Priority Registers (4-7, 8-11, 12-15) */
+  __IO uint32_t SHCSR;                   /*!< Offset: 0x024 (R/W)  System Handler Control and State Register             */
+  __IO uint32_t CFSR;                    /*!< Offset: 0x028 (R/W)  Configurable Fault Status Register                    */
+  __IO uint32_t HFSR;                    /*!< Offset: 0x02C (R/W)  HardFault Status Register                             */
+  __IO uint32_t DFSR;                    /*!< Offset: 0x030 (R/W)  Debug Fault Status Register                           */
+  __IO uint32_t MMFAR;                   /*!< Offset: 0x034 (R/W)  MemManage Fault Address Register                      */
+  __IO uint32_t BFAR;                    /*!< Offset: 0x038 (R/W)  BusFault Address Register                             */
+  __IO uint32_t AFSR;                    /*!< Offset: 0x03C (R/W)  Auxiliary Fault Status Register                       */
+  __I  uint32_t PFR[2];                  /*!< Offset: 0x040 (R/ )  Processor Feature Register                            */
+  __I  uint32_t DFR;                     /*!< Offset: 0x048 (R/ )  Debug Feature Register                                */
+  __I  uint32_t ADR;                     /*!< Offset: 0x04C (R/ )  Auxiliary Feature Register                            */
+  __I  uint32_t MMFR[4];                 /*!< Offset: 0x050 (R/ )  Memory Model Feature Register                         */
+  __I  uint32_t ISAR[5];                 /*!< Offset: 0x060 (R/ )  Instruction Set Attributes Register                   */
+       uint32_t RESERVED0[5];
+  __IO uint32_t CPACR;                   /*!< Offset: 0x088 (R/W)  Coprocessor Access Control Register                   */
+} SCB_Type;
+
+/* SCB CPUID Register Definitions */
+#define SCB_CPUID_IMPLEMENTER_Pos          24                                             /*!< SCB CPUID: IMPLEMENTER Position */
+#define SCB_CPUID_IMPLEMENTER_Msk          (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos)          /*!< SCB CPUID: IMPLEMENTER Mask */
+
+#define SCB_CPUID_VARIANT_Pos              20                                             /*!< SCB CPUID: VARIANT Position */
+#define SCB_CPUID_VARIANT_Msk              (0xFUL << SCB_CPUID_VARIANT_Pos)               /*!< SCB CPUID: VARIANT Mask */
+
+#define SCB_CPUID_ARCHITECTURE_Pos         16                                             /*!< SCB CPUID: ARCHITECTURE Position */
+#define SCB_CPUID_ARCHITECTURE_Msk         (0xFUL << SCB_CPUID_ARCHITECTURE_Pos)          /*!< SCB CPUID: ARCHITECTURE Mask */
+
+#define SCB_CPUID_PARTNO_Pos                4                                             /*!< SCB CPUID: PARTNO Position */
+#define SCB_CPUID_PARTNO_Msk               (0xFFFUL << SCB_CPUID_PARTNO_Pos)              /*!< SCB CPUID: PARTNO Mask */
+
+#define SCB_CPUID_REVISION_Pos              0                                             /*!< SCB CPUID: REVISION Position */
+#define SCB_CPUID_REVISION_Msk             (0xFUL << SCB_CPUID_REVISION_Pos)              /*!< SCB CPUID: REVISION Mask */
+
+/* SCB Interrupt Control State Register Definitions */
+#define SCB_ICSR_NMIPENDSET_Pos            31                                             /*!< SCB ICSR: NMIPENDSET Position */
+#define SCB_ICSR_NMIPENDSET_Msk            (1UL << SCB_ICSR_NMIPENDSET_Pos)               /*!< SCB ICSR: NMIPENDSET Mask */
+
+#define SCB_ICSR_PENDSVSET_Pos             28                                             /*!< SCB ICSR: PENDSVSET Position */
+#define SCB_ICSR_PENDSVSET_Msk             (1UL << SCB_ICSR_PENDSVSET_Pos)                /*!< SCB ICSR: PENDSVSET Mask */
+
+#define SCB_ICSR_PENDSVCLR_Pos             27                                             /*!< SCB ICSR: PENDSVCLR Position */
+#define SCB_ICSR_PENDSVCLR_Msk             (1UL << SCB_ICSR_PENDSVCLR_Pos)                /*!< SCB ICSR: PENDSVCLR Mask */
+
+#define SCB_ICSR_PENDSTSET_Pos             26                                             /*!< SCB ICSR: PENDSTSET Position */
+#define SCB_ICSR_PENDSTSET_Msk             (1UL << SCB_ICSR_PENDSTSET_Pos)                /*!< SCB ICSR: PENDSTSET Mask */
+
+#define SCB_ICSR_PENDSTCLR_Pos             25                                             /*!< SCB ICSR: PENDSTCLR Position */
+#define SCB_ICSR_PENDSTCLR_Msk             (1UL << SCB_ICSR_PENDSTCLR_Pos)                /*!< SCB ICSR: PENDSTCLR Mask */
+
+#define SCB_ICSR_ISRPREEMPT_Pos            23                                             /*!< SCB ICSR: ISRPREEMPT Position */
+#define SCB_ICSR_ISRPREEMPT_Msk            (1UL << SCB_ICSR_ISRPREEMPT_Pos)               /*!< SCB ICSR: ISRPREEMPT Mask */
+
+#define SCB_ICSR_ISRPENDING_Pos            22                                             /*!< SCB ICSR: ISRPENDING Position */
+#define SCB_ICSR_ISRPENDING_Msk            (1UL << SCB_ICSR_ISRPENDING_Pos)               /*!< SCB ICSR: ISRPENDING Mask */
+
+#define SCB_ICSR_VECTPENDING_Pos           12                                             /*!< SCB ICSR: VECTPENDING Position */
+#define SCB_ICSR_VECTPENDING_Msk           (0x1FFUL << SCB_ICSR_VECTPENDING_Pos)          /*!< SCB ICSR: VECTPENDING Mask */
+
+#define SCB_ICSR_RETTOBASE_Pos             11                                             /*!< SCB ICSR: RETTOBASE Position */
+#define SCB_ICSR_RETTOBASE_Msk             (1UL << SCB_ICSR_RETTOBASE_Pos)                /*!< SCB ICSR: RETTOBASE Mask */
+
+#define SCB_ICSR_VECTACTIVE_Pos             0                                             /*!< SCB ICSR: VECTACTIVE Position */
+#define SCB_ICSR_VECTACTIVE_Msk            (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos)           /*!< SCB ICSR: VECTACTIVE Mask */
+
+/* SCB Vector Table Offset Register Definitions */
+#define SCB_VTOR_TBLOFF_Pos                 7                                             /*!< SCB VTOR: TBLOFF Position */
+#define SCB_VTOR_TBLOFF_Msk                (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos)           /*!< SCB VTOR: TBLOFF Mask */
+
+/* SCB Application Interrupt and Reset Control Register Definitions */
+#define SCB_AIRCR_VECTKEY_Pos              16                                             /*!< SCB AIRCR: VECTKEY Position */
+#define SCB_AIRCR_VECTKEY_Msk              (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos)            /*!< SCB AIRCR: VECTKEY Mask */
+
+#define SCB_AIRCR_VECTKEYSTAT_Pos          16                                             /*!< SCB AIRCR: VECTKEYSTAT Position */
+#define SCB_AIRCR_VECTKEYSTAT_Msk          (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos)        /*!< SCB AIRCR: VECTKEYSTAT Mask */
+
+#define SCB_AIRCR_ENDIANESS_Pos            15                                             /*!< SCB AIRCR: ENDIANESS Position */
+#define SCB_AIRCR_ENDIANESS_Msk            (1UL << SCB_AIRCR_ENDIANESS_Pos)               /*!< SCB AIRCR: ENDIANESS Mask */
+
+#define SCB_AIRCR_PRIGROUP_Pos              8                                             /*!< SCB AIRCR: PRIGROUP Position */
+#define SCB_AIRCR_PRIGROUP_Msk             (7UL << SCB_AIRCR_PRIGROUP_Pos)                /*!< SCB AIRCR: PRIGROUP Mask */
+
+#define SCB_AIRCR_SYSRESETREQ_Pos           2                                             /*!< SCB AIRCR: SYSRESETREQ Position */
+#define SCB_AIRCR_SYSRESETREQ_Msk          (1UL << SCB_AIRCR_SYSRESETREQ_Pos)             /*!< SCB AIRCR: SYSRESETREQ Mask */
+
+#define SCB_AIRCR_VECTCLRACTIVE_Pos         1                                             /*!< SCB AIRCR: VECTCLRACTIVE Position */
+#define SCB_AIRCR_VECTCLRACTIVE_Msk        (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos)           /*!< SCB AIRCR: VECTCLRACTIVE Mask */
+
+#define SCB_AIRCR_VECTRESET_Pos             0                                             /*!< SCB AIRCR: VECTRESET Position */
+#define SCB_AIRCR_VECTRESET_Msk            (1UL << SCB_AIRCR_VECTRESET_Pos)               /*!< SCB AIRCR: VECTRESET Mask */
+
+/* SCB System Control Register Definitions */
+#define SCB_SCR_SEVONPEND_Pos               4                                             /*!< SCB SCR: SEVONPEND Position */
+#define SCB_SCR_SEVONPEND_Msk              (1UL << SCB_SCR_SEVONPEND_Pos)                 /*!< SCB SCR: SEVONPEND Mask */
+
+#define SCB_SCR_SLEEPDEEP_Pos               2                                             /*!< SCB SCR: SLEEPDEEP Position */
+#define SCB_SCR_SLEEPDEEP_Msk              (1UL << SCB_SCR_SLEEPDEEP_Pos)                 /*!< SCB SCR: SLEEPDEEP Mask */
+
+#define SCB_SCR_SLEEPONEXIT_Pos             1                                             /*!< SCB SCR: SLEEPONEXIT Position */
+#define SCB_SCR_SLEEPONEXIT_Msk            (1UL << SCB_SCR_SLEEPONEXIT_Pos)               /*!< SCB SCR: SLEEPONEXIT Mask */
+
+/* SCB Configuration Control Register Definitions */
+#define SCB_CCR_STKALIGN_Pos                9                                             /*!< SCB CCR: STKALIGN Position */
+#define SCB_CCR_STKALIGN_Msk               (1UL << SCB_CCR_STKALIGN_Pos)                  /*!< SCB CCR: STKALIGN Mask */
+
+#define SCB_CCR_BFHFNMIGN_Pos               8                                             /*!< SCB CCR: BFHFNMIGN Position */
+#define SCB_CCR_BFHFNMIGN_Msk              (1UL << SCB_CCR_BFHFNMIGN_Pos)                 /*!< SCB CCR: BFHFNMIGN Mask */
+
+#define SCB_CCR_DIV_0_TRP_Pos               4                                             /*!< SCB CCR: DIV_0_TRP Position */
+#define SCB_CCR_DIV_0_TRP_Msk              (1UL << SCB_CCR_DIV_0_TRP_Pos)                 /*!< SCB CCR: DIV_0_TRP Mask */
+
+#define SCB_CCR_UNALIGN_TRP_Pos             3                                             /*!< SCB CCR: UNALIGN_TRP Position */
+#define SCB_CCR_UNALIGN_TRP_Msk            (1UL << SCB_CCR_UNALIGN_TRP_Pos)               /*!< SCB CCR: UNALIGN_TRP Mask */
+
+#define SCB_CCR_USERSETMPEND_Pos            1                                             /*!< SCB CCR: USERSETMPEND Position */
+#define SCB_CCR_USERSETMPEND_Msk           (1UL << SCB_CCR_USERSETMPEND_Pos)              /*!< SCB CCR: USERSETMPEND Mask */
+
+#define SCB_CCR_NONBASETHRDENA_Pos          0                                             /*!< SCB CCR: NONBASETHRDENA Position */
+#define SCB_CCR_NONBASETHRDENA_Msk         (1UL << SCB_CCR_NONBASETHRDENA_Pos)            /*!< SCB CCR: NONBASETHRDENA Mask */
+
+/* SCB System Handler Control and State Register Definitions */
+#define SCB_SHCSR_USGFAULTENA_Pos          18                                             /*!< SCB SHCSR: USGFAULTENA Position */
+#define SCB_SHCSR_USGFAULTENA_Msk          (1UL << SCB_SHCSR_USGFAULTENA_Pos)             /*!< SCB SHCSR: USGFAULTENA Mask */
+
+#define SCB_SHCSR_BUSFAULTENA_Pos          17                                             /*!< SCB SHCSR: BUSFAULTENA Position */
+#define SCB_SHCSR_BUSFAULTENA_Msk          (1UL << SCB_SHCSR_BUSFAULTENA_Pos)             /*!< SCB SHCSR: BUSFAULTENA Mask */
+
+#define SCB_SHCSR_MEMFAULTENA_Pos          16                                             /*!< SCB SHCSR: MEMFAULTENA Position */
+#define SCB_SHCSR_MEMFAULTENA_Msk          (1UL << SCB_SHCSR_MEMFAULTENA_Pos)             /*!< SCB SHCSR: MEMFAULTENA Mask */
+
+#define SCB_SHCSR_SVCALLPENDED_Pos         15                                             /*!< SCB SHCSR: SVCALLPENDED Position */
+#define SCB_SHCSR_SVCALLPENDED_Msk         (1UL << SCB_SHCSR_SVCALLPENDED_Pos)            /*!< SCB SHCSR: SVCALLPENDED Mask */
+
+#define SCB_SHCSR_BUSFAULTPENDED_Pos       14                                             /*!< SCB SHCSR: BUSFAULTPENDED Position */
+#define SCB_SHCSR_BUSFAULTPENDED_Msk       (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos)          /*!< SCB SHCSR: BUSFAULTPENDED Mask */
+
+#define SCB_SHCSR_MEMFAULTPENDED_Pos       13                                             /*!< SCB SHCSR: MEMFAULTPENDED Position */
+#define SCB_SHCSR_MEMFAULTPENDED_Msk       (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos)          /*!< SCB SHCSR: MEMFAULTPENDED Mask */
+
+#define SCB_SHCSR_USGFAULTPENDED_Pos       12                                             /*!< SCB SHCSR: USGFAULTPENDED Position */
+#define SCB_SHCSR_USGFAULTPENDED_Msk       (1UL << SCB_SHCSR_USGFAULTPENDED_Pos)          /*!< SCB SHCSR: USGFAULTPENDED Mask */
+
+#define SCB_SHCSR_SYSTICKACT_Pos           11                                             /*!< SCB SHCSR: SYSTICKACT Position */
+#define SCB_SHCSR_SYSTICKACT_Msk           (1UL << SCB_SHCSR_SYSTICKACT_Pos)              /*!< SCB SHCSR: SYSTICKACT Mask */
+
+#define SCB_SHCSR_PENDSVACT_Pos            10                                             /*!< SCB SHCSR: PENDSVACT Position */
+#define SCB_SHCSR_PENDSVACT_Msk            (1UL << SCB_SHCSR_PENDSVACT_Pos)               /*!< SCB SHCSR: PENDSVACT Mask */
+
+#define SCB_SHCSR_MONITORACT_Pos            8                                             /*!< SCB SHCSR: MONITORACT Position */
+#define SCB_SHCSR_MONITORACT_Msk           (1UL << SCB_SHCSR_MONITORACT_Pos)              /*!< SCB SHCSR: MONITORACT Mask */
+
+#define SCB_SHCSR_SVCALLACT_Pos             7                                             /*!< SCB SHCSR: SVCALLACT Position */
+#define SCB_SHCSR_SVCALLACT_Msk            (1UL << SCB_SHCSR_SVCALLACT_Pos)               /*!< SCB SHCSR: SVCALLACT Mask */
+
+#define SCB_SHCSR_USGFAULTACT_Pos           3                                             /*!< SCB SHCSR: USGFAULTACT Position */
+#define SCB_SHCSR_USGFAULTACT_Msk          (1UL << SCB_SHCSR_USGFAULTACT_Pos)             /*!< SCB SHCSR: USGFAULTACT Mask */
+
+#define SCB_SHCSR_BUSFAULTACT_Pos           1                                             /*!< SCB SHCSR: BUSFAULTACT Position */
+#define SCB_SHCSR_BUSFAULTACT_Msk          (1UL << SCB_SHCSR_BUSFAULTACT_Pos)             /*!< SCB SHCSR: BUSFAULTACT Mask */
+
+#define SCB_SHCSR_MEMFAULTACT_Pos           0                                             /*!< SCB SHCSR: MEMFAULTACT Position */
+#define SCB_SHCSR_MEMFAULTACT_Msk          (1UL << SCB_SHCSR_MEMFAULTACT_Pos)             /*!< SCB SHCSR: MEMFAULTACT Mask */
+
+/* SCB Configurable Fault Status Registers Definitions */
+#define SCB_CFSR_USGFAULTSR_Pos            16                                             /*!< SCB CFSR: Usage Fault Status Register Position */
+#define SCB_CFSR_USGFAULTSR_Msk            (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos)          /*!< SCB CFSR: Usage Fault Status Register Mask */
+
+#define SCB_CFSR_BUSFAULTSR_Pos             8                                             /*!< SCB CFSR: Bus Fault Status Register Position */
+#define SCB_CFSR_BUSFAULTSR_Msk            (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos)            /*!< SCB CFSR: Bus Fault Status Register Mask */
+
+#define SCB_CFSR_MEMFAULTSR_Pos             0                                             /*!< SCB CFSR: Memory Manage Fault Status Register Position */
+#define SCB_CFSR_MEMFAULTSR_Msk            (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos)            /*!< SCB CFSR: Memory Manage Fault Status Register Mask */
+
+/* SCB Hard Fault Status Registers Definitions */
+#define SCB_HFSR_DEBUGEVT_Pos              31                                             /*!< SCB HFSR: DEBUGEVT Position */
+#define SCB_HFSR_DEBUGEVT_Msk              (1UL << SCB_HFSR_DEBUGEVT_Pos)                 /*!< SCB HFSR: DEBUGEVT Mask */
+
+#define SCB_HFSR_FORCED_Pos                30                                             /*!< SCB HFSR: FORCED Position */
+#define SCB_HFSR_FORCED_Msk                (1UL << SCB_HFSR_FORCED_Pos)                   /*!< SCB HFSR: FORCED Mask */
+
+#define SCB_HFSR_VECTTBL_Pos                1                                             /*!< SCB HFSR: VECTTBL Position */
+#define SCB_HFSR_VECTTBL_Msk               (1UL << SCB_HFSR_VECTTBL_Pos)                  /*!< SCB HFSR: VECTTBL Mask */
+
+/* SCB Debug Fault Status Register Definitions */
+#define SCB_DFSR_EXTERNAL_Pos               4                                             /*!< SCB DFSR: EXTERNAL Position */
+#define SCB_DFSR_EXTERNAL_Msk              (1UL << SCB_DFSR_EXTERNAL_Pos)                 /*!< SCB DFSR: EXTERNAL Mask */
+
+#define SCB_DFSR_VCATCH_Pos                 3                                             /*!< SCB DFSR: VCATCH Position */
+#define SCB_DFSR_VCATCH_Msk                (1UL << SCB_DFSR_VCATCH_Pos)                   /*!< SCB DFSR: VCATCH Mask */
+
+#define SCB_DFSR_DWTTRAP_Pos                2                                             /*!< SCB DFSR: DWTTRAP Position */
+#define SCB_DFSR_DWTTRAP_Msk               (1UL << SCB_DFSR_DWTTRAP_Pos)                  /*!< SCB DFSR: DWTTRAP Mask */
+
+#define SCB_DFSR_BKPT_Pos                   1                                             /*!< SCB DFSR: BKPT Position */
+#define SCB_DFSR_BKPT_Msk                  (1UL << SCB_DFSR_BKPT_Pos)                     /*!< SCB DFSR: BKPT Mask */
+
+#define SCB_DFSR_HALTED_Pos                 0                                             /*!< SCB DFSR: HALTED Position */
+#define SCB_DFSR_HALTED_Msk                (1UL << SCB_DFSR_HALTED_Pos)                   /*!< SCB DFSR: HALTED Mask */
+
+/*@} end of group CMSIS_SCB */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)
+    \brief      Type definitions for the System Control and ID Register not in the SCB
+  @{
+ */
+
+/** \brief  Structure type to access the System Control and ID Register not in the SCB.
+ */
+typedef struct
+{
+       uint32_t RESERVED0[1];
+  __I  uint32_t ICTR;                    /*!< Offset: 0x004 (R/ )  Interrupt Controller Type Register      */
+  __IO uint32_t ACTLR;                   /*!< Offset: 0x008 (R/W)  Auxiliary Control Register              */
+} SCnSCB_Type;
+
+/* Interrupt Controller Type Register Definitions */
+#define SCnSCB_ICTR_INTLINESNUM_Pos         0                                          /*!< ICTR: INTLINESNUM Position */
+#define SCnSCB_ICTR_INTLINESNUM_Msk        (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos)      /*!< ICTR: INTLINESNUM Mask */
+
+/* Auxiliary Control Register Definitions */
+#define SCnSCB_ACTLR_DISOOFP_Pos            9                                          /*!< ACTLR: DISOOFP Position */
+#define SCnSCB_ACTLR_DISOOFP_Msk           (1UL << SCnSCB_ACTLR_DISOOFP_Pos)           /*!< ACTLR: DISOOFP Mask */
+
+#define SCnSCB_ACTLR_DISFPCA_Pos            8                                          /*!< ACTLR: DISFPCA Position */
+#define SCnSCB_ACTLR_DISFPCA_Msk           (1UL << SCnSCB_ACTLR_DISFPCA_Pos)           /*!< ACTLR: DISFPCA Mask */
+
+#define SCnSCB_ACTLR_DISFOLD_Pos            2                                          /*!< ACTLR: DISFOLD Position */
+#define SCnSCB_ACTLR_DISFOLD_Msk           (1UL << SCnSCB_ACTLR_DISFOLD_Pos)           /*!< ACTLR: DISFOLD Mask */
+
+#define SCnSCB_ACTLR_DISDEFWBUF_Pos         1                                          /*!< ACTLR: DISDEFWBUF Position */
+#define SCnSCB_ACTLR_DISDEFWBUF_Msk        (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos)        /*!< ACTLR: DISDEFWBUF Mask */
+
+#define SCnSCB_ACTLR_DISMCYCINT_Pos         0                                          /*!< ACTLR: DISMCYCINT Position */
+#define SCnSCB_ACTLR_DISMCYCINT_Msk        (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos)        /*!< ACTLR: DISMCYCINT Mask */
+
+/*@} end of group CMSIS_SCnotSCB */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_SysTick     System Tick Timer (SysTick)
+    \brief      Type definitions for the System Timer Registers.
+  @{
+ */
+
+/** \brief  Structure type to access the System Timer (SysTick).
+ */
+typedef struct
+{
+  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  SysTick Control and Status Register */
+  __IO uint32_t LOAD;                    /*!< Offset: 0x004 (R/W)  SysTick Reload Value Register       */
+  __IO uint32_t VAL;                     /*!< Offset: 0x008 (R/W)  SysTick Current Value Register      */
+  __I  uint32_t CALIB;                   /*!< Offset: 0x00C (R/ )  SysTick Calibration Register        */
+} SysTick_Type;
+
+/* SysTick Control / Status Register Definitions */
+#define SysTick_CTRL_COUNTFLAG_Pos         16                                             /*!< SysTick CTRL: COUNTFLAG Position */
+#define SysTick_CTRL_COUNTFLAG_Msk         (1UL << SysTick_CTRL_COUNTFLAG_Pos)            /*!< SysTick CTRL: COUNTFLAG Mask */
+
+#define SysTick_CTRL_CLKSOURCE_Pos          2                                             /*!< SysTick CTRL: CLKSOURCE Position */
+#define SysTick_CTRL_CLKSOURCE_Msk         (1UL << SysTick_CTRL_CLKSOURCE_Pos)            /*!< SysTick CTRL: CLKSOURCE Mask */
+
+#define SysTick_CTRL_TICKINT_Pos            1                                             /*!< SysTick CTRL: TICKINT Position */
+#define SysTick_CTRL_TICKINT_Msk           (1UL << SysTick_CTRL_TICKINT_Pos)              /*!< SysTick CTRL: TICKINT Mask */
+
+#define SysTick_CTRL_ENABLE_Pos             0                                             /*!< SysTick CTRL: ENABLE Position */
+#define SysTick_CTRL_ENABLE_Msk            (1UL << SysTick_CTRL_ENABLE_Pos)               /*!< SysTick CTRL: ENABLE Mask */
+
+/* SysTick Reload Register Definitions */
+#define SysTick_LOAD_RELOAD_Pos             0                                             /*!< SysTick LOAD: RELOAD Position */
+#define SysTick_LOAD_RELOAD_Msk            (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos)        /*!< SysTick LOAD: RELOAD Mask */
+
+/* SysTick Current Register Definitions */
+#define SysTick_VAL_CURRENT_Pos             0                                             /*!< SysTick VAL: CURRENT Position */
+#define SysTick_VAL_CURRENT_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick VAL: CURRENT Mask */
+
+/* SysTick Calibration Register Definitions */
+#define SysTick_CALIB_NOREF_Pos            31                                             /*!< SysTick CALIB: NOREF Position */
+#define SysTick_CALIB_NOREF_Msk            (1UL << SysTick_CALIB_NOREF_Pos)               /*!< SysTick CALIB: NOREF Mask */
+
+#define SysTick_CALIB_SKEW_Pos             30                                             /*!< SysTick CALIB: SKEW Position */
+#define SysTick_CALIB_SKEW_Msk             (1UL << SysTick_CALIB_SKEW_Pos)                /*!< SysTick CALIB: SKEW Mask */
+
+#define SysTick_CALIB_TENMS_Pos             0                                             /*!< SysTick CALIB: TENMS Position */
+#define SysTick_CALIB_TENMS_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick CALIB: TENMS Mask */
+
+/*@} end of group CMSIS_SysTick */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_ITM     Instrumentation Trace Macrocell (ITM)
+    \brief      Type definitions for the Instrumentation Trace Macrocell (ITM)
+  @{
+ */
+
+/** \brief  Structure type to access the Instrumentation Trace Macrocell Register (ITM).
+ */
+typedef struct
+{
+  __O  union
+  {
+    __O  uint8_t    u8;                  /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 8-bit                   */
+    __O  uint16_t   u16;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 16-bit                  */
+    __O  uint32_t   u32;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 32-bit                  */
+  }  PORT [32];                          /*!< Offset: 0x000 ( /W)  ITM Stimulus Port Registers               */
+       uint32_t RESERVED0[864];
+  __IO uint32_t TER;                     /*!< Offset: 0xE00 (R/W)  ITM Trace Enable Register                 */
+       uint32_t RESERVED1[15];
+  __IO uint32_t TPR;                     /*!< Offset: 0xE40 (R/W)  ITM Trace Privilege Register              */
+       uint32_t RESERVED2[15];
+  __IO uint32_t TCR;                     /*!< Offset: 0xE80 (R/W)  ITM Trace Control Register                */
+       uint32_t RESERVED3[29];
+  __O  uint32_t IWR;                     /*!< Offset: 0xEF8 ( /W)  ITM Integration Write Register            */
+  __I  uint32_t IRR;                     /*!< Offset: 0xEFC (R/ )  ITM Integration Read Register             */
+  __IO uint32_t IMCR;                    /*!< Offset: 0xF00 (R/W)  ITM Integration Mode Control Register     */
+       uint32_t RESERVED4[43];
+  __O  uint32_t LAR;                     /*!< Offset: 0xFB0 ( /W)  ITM Lock Access Register                  */
+  __I  uint32_t LSR;                     /*!< Offset: 0xFB4 (R/ )  ITM Lock Status Register                  */
+       uint32_t RESERVED5[6];
+  __I  uint32_t PID4;                    /*!< Offset: 0xFD0 (R/ )  ITM Peripheral Identification Register #4 */
+  __I  uint32_t PID5;                    /*!< Offset: 0xFD4 (R/ )  ITM Peripheral Identification Register #5 */
+  __I  uint32_t PID6;                    /*!< Offset: 0xFD8 (R/ )  ITM Peripheral Identification Register #6 */
+  __I  uint32_t PID7;                    /*!< Offset: 0xFDC (R/ )  ITM Peripheral Identification Register #7 */
+  __I  uint32_t PID0;                    /*!< Offset: 0xFE0 (R/ )  ITM Peripheral Identification Register #0 */
+  __I  uint32_t PID1;                    /*!< Offset: 0xFE4 (R/ )  ITM Peripheral Identification Register #1 */
+  __I  uint32_t PID2;                    /*!< Offset: 0xFE8 (R/ )  ITM Peripheral Identification Register #2 */
+  __I  uint32_t PID3;                    /*!< Offset: 0xFEC (R/ )  ITM Peripheral Identification Register #3 */
+  __I  uint32_t CID0;                    /*!< Offset: 0xFF0 (R/ )  ITM Component  Identification Register #0 */
+  __I  uint32_t CID1;                    /*!< Offset: 0xFF4 (R/ )  ITM Component  Identification Register #1 */
+  __I  uint32_t CID2;                    /*!< Offset: 0xFF8 (R/ )  ITM Component  Identification Register #2 */
+  __I  uint32_t CID3;                    /*!< Offset: 0xFFC (R/ )  ITM Component  Identification Register #3 */
+} ITM_Type;
+
+/* ITM Trace Privilege Register Definitions */
+#define ITM_TPR_PRIVMASK_Pos                0                                             /*!< ITM TPR: PRIVMASK Position */
+#define ITM_TPR_PRIVMASK_Msk               (0xFUL << ITM_TPR_PRIVMASK_Pos)                /*!< ITM TPR: PRIVMASK Mask */
+
+/* ITM Trace Control Register Definitions */
+#define ITM_TCR_BUSY_Pos                   23                                             /*!< ITM TCR: BUSY Position */
+#define ITM_TCR_BUSY_Msk                   (1UL << ITM_TCR_BUSY_Pos)                      /*!< ITM TCR: BUSY Mask */
+
+#define ITM_TCR_TraceBusID_Pos             16                                             /*!< ITM TCR: ATBID Position */
+#define ITM_TCR_TraceBusID_Msk             (0x7FUL << ITM_TCR_TraceBusID_Pos)             /*!< ITM TCR: ATBID Mask */
+
+#define ITM_TCR_GTSFREQ_Pos                10                                             /*!< ITM TCR: Global timestamp frequency Position */
+#define ITM_TCR_GTSFREQ_Msk                (3UL << ITM_TCR_GTSFREQ_Pos)                   /*!< ITM TCR: Global timestamp frequency Mask */
+
+#define ITM_TCR_TSPrescale_Pos              8                                             /*!< ITM TCR: TSPrescale Position */
+#define ITM_TCR_TSPrescale_Msk             (3UL << ITM_TCR_TSPrescale_Pos)                /*!< ITM TCR: TSPrescale Mask */
+
+#define ITM_TCR_SWOENA_Pos                  4                                             /*!< ITM TCR: SWOENA Position */
+#define ITM_TCR_SWOENA_Msk                 (1UL << ITM_TCR_SWOENA_Pos)                    /*!< ITM TCR: SWOENA Mask */
+
+#define ITM_TCR_DWTENA_Pos                  3                                             /*!< ITM TCR: DWTENA Position */
+#define ITM_TCR_DWTENA_Msk                 (1UL << ITM_TCR_DWTENA_Pos)                    /*!< ITM TCR: DWTENA Mask */
+
+#define ITM_TCR_SYNCENA_Pos                 2                                             /*!< ITM TCR: SYNCENA Position */
+#define ITM_TCR_SYNCENA_Msk                (1UL << ITM_TCR_SYNCENA_Pos)                   /*!< ITM TCR: SYNCENA Mask */
+
+#define ITM_TCR_TSENA_Pos                   1                                             /*!< ITM TCR: TSENA Position */
+#define ITM_TCR_TSENA_Msk                  (1UL << ITM_TCR_TSENA_Pos)                     /*!< ITM TCR: TSENA Mask */
+
+#define ITM_TCR_ITMENA_Pos                  0                                             /*!< ITM TCR: ITM Enable bit Position */
+#define ITM_TCR_ITMENA_Msk                 (1UL << ITM_TCR_ITMENA_Pos)                    /*!< ITM TCR: ITM Enable bit Mask */
+
+/* ITM Integration Write Register Definitions */
+#define ITM_IWR_ATVALIDM_Pos                0                                             /*!< ITM IWR: ATVALIDM Position */
+#define ITM_IWR_ATVALIDM_Msk               (1UL << ITM_IWR_ATVALIDM_Pos)                  /*!< ITM IWR: ATVALIDM Mask */
+
+/* ITM Integration Read Register Definitions */
+#define ITM_IRR_ATREADYM_Pos                0                                             /*!< ITM IRR: ATREADYM Position */
+#define ITM_IRR_ATREADYM_Msk               (1UL << ITM_IRR_ATREADYM_Pos)                  /*!< ITM IRR: ATREADYM Mask */
+
+/* ITM Integration Mode Control Register Definitions */
+#define ITM_IMCR_INTEGRATION_Pos            0                                             /*!< ITM IMCR: INTEGRATION Position */
+#define ITM_IMCR_INTEGRATION_Msk           (1UL << ITM_IMCR_INTEGRATION_Pos)              /*!< ITM IMCR: INTEGRATION Mask */
+
+/* ITM Lock Status Register Definitions */
+#define ITM_LSR_ByteAcc_Pos                 2                                             /*!< ITM LSR: ByteAcc Position */
+#define ITM_LSR_ByteAcc_Msk                (1UL << ITM_LSR_ByteAcc_Pos)                   /*!< ITM LSR: ByteAcc Mask */
+
+#define ITM_LSR_Access_Pos                  1                                             /*!< ITM LSR: Access Position */
+#define ITM_LSR_Access_Msk                 (1UL << ITM_LSR_Access_Pos)                    /*!< ITM LSR: Access Mask */
+
+#define ITM_LSR_Present_Pos                 0                                             /*!< ITM LSR: Present Position */
+#define ITM_LSR_Present_Msk                (1UL << ITM_LSR_Present_Pos)                   /*!< ITM LSR: Present Mask */
+
+/*@}*/ /* end of group CMSIS_ITM */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_DWT     Data Watchpoint and Trace (DWT)
+    \brief      Type definitions for the Data Watchpoint and Trace (DWT)
+  @{
+ */
+
+/** \brief  Structure type to access the Data Watchpoint and Trace Register (DWT).
+ */
+typedef struct
+{
+  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  Control Register                          */
+  __IO uint32_t CYCCNT;                  /*!< Offset: 0x004 (R/W)  Cycle Count Register                      */
+  __IO uint32_t CPICNT;                  /*!< Offset: 0x008 (R/W)  CPI Count Register                        */
+  __IO uint32_t EXCCNT;                  /*!< Offset: 0x00C (R/W)  Exception Overhead Count Register         */
+  __IO uint32_t SLEEPCNT;                /*!< Offset: 0x010 (R/W)  Sleep Count Register                      */
+  __IO uint32_t LSUCNT;                  /*!< Offset: 0x014 (R/W)  LSU Count Register                        */
+  __IO uint32_t FOLDCNT;                 /*!< Offset: 0x018 (R/W)  Folded-instruction Count Register         */
+  __I  uint32_t PCSR;                    /*!< Offset: 0x01C (R/ )  Program Counter Sample Register           */
+  __IO uint32_t COMP0;                   /*!< Offset: 0x020 (R/W)  Comparator Register 0                     */
+  __IO uint32_t MASK0;                   /*!< Offset: 0x024 (R/W)  Mask Register 0                           */
+  __IO uint32_t FUNCTION0;               /*!< Offset: 0x028 (R/W)  Function Register 0                       */
+       uint32_t RESERVED0[1];
+  __IO uint32_t COMP1;                   /*!< Offset: 0x030 (R/W)  Comparator Register 1                     */
+  __IO uint32_t MASK1;                   /*!< Offset: 0x034 (R/W)  Mask Register 1                           */
+  __IO uint32_t FUNCTION1;               /*!< Offset: 0x038 (R/W)  Function Register 1                       */
+       uint32_t RESERVED1[1];
+  __IO uint32_t COMP2;                   /*!< Offset: 0x040 (R/W)  Comparator Register 2                     */
+  __IO uint32_t MASK2;                   /*!< Offset: 0x044 (R/W)  Mask Register 2                           */
+  __IO uint32_t FUNCTION2;               /*!< Offset: 0x048 (R/W)  Function Register 2                       */
+       uint32_t RESERVED2[1];
+  __IO uint32_t COMP3;                   /*!< Offset: 0x050 (R/W)  Comparator Register 3                     */
+  __IO uint32_t MASK3;                   /*!< Offset: 0x054 (R/W)  Mask Register 3                           */
+  __IO uint32_t FUNCTION3;               /*!< Offset: 0x058 (R/W)  Function Register 3                       */
+} DWT_Type;
+
+/* DWT Control Register Definitions */
+#define DWT_CTRL_NUMCOMP_Pos               28                                          /*!< DWT CTRL: NUMCOMP Position */
+#define DWT_CTRL_NUMCOMP_Msk               (0xFUL << DWT_CTRL_NUMCOMP_Pos)             /*!< DWT CTRL: NUMCOMP Mask */
+
+#define DWT_CTRL_NOTRCPKT_Pos              27                                          /*!< DWT CTRL: NOTRCPKT Position */
+#define DWT_CTRL_NOTRCPKT_Msk              (0x1UL << DWT_CTRL_NOTRCPKT_Pos)            /*!< DWT CTRL: NOTRCPKT Mask */
+
+#define DWT_CTRL_NOEXTTRIG_Pos             26                                          /*!< DWT CTRL: NOEXTTRIG Position */
+#define DWT_CTRL_NOEXTTRIG_Msk             (0x1UL << DWT_CTRL_NOEXTTRIG_Pos)           /*!< DWT CTRL: NOEXTTRIG Mask */
+
+#define DWT_CTRL_NOCYCCNT_Pos              25                                          /*!< DWT CTRL: NOCYCCNT Position */
+#define DWT_CTRL_NOCYCCNT_Msk              (0x1UL << DWT_CTRL_NOCYCCNT_Pos)            /*!< DWT CTRL: NOCYCCNT Mask */
+
+#define DWT_CTRL_NOPRFCNT_Pos              24                                          /*!< DWT CTRL: NOPRFCNT Position */
+#define DWT_CTRL_NOPRFCNT_Msk              (0x1UL << DWT_CTRL_NOPRFCNT_Pos)            /*!< DWT CTRL: NOPRFCNT Mask */
+
+#define DWT_CTRL_CYCEVTENA_Pos             22                                          /*!< DWT CTRL: CYCEVTENA Position */
+#define DWT_CTRL_CYCEVTENA_Msk             (0x1UL << DWT_CTRL_CYCEVTENA_Pos)           /*!< DWT CTRL: CYCEVTENA Mask */
+
+#define DWT_CTRL_FOLDEVTENA_Pos            21                                          /*!< DWT CTRL: FOLDEVTENA Position */
+#define DWT_CTRL_FOLDEVTENA_Msk            (0x1UL << DWT_CTRL_FOLDEVTENA_Pos)          /*!< DWT CTRL: FOLDEVTENA Mask */
+
+#define DWT_CTRL_LSUEVTENA_Pos             20                                          /*!< DWT CTRL: LSUEVTENA Position */
+#define DWT_CTRL_LSUEVTENA_Msk             (0x1UL << DWT_CTRL_LSUEVTENA_Pos)           /*!< DWT CTRL: LSUEVTENA Mask */
+
+#define DWT_CTRL_SLEEPEVTENA_Pos           19                                          /*!< DWT CTRL: SLEEPEVTENA Position */
+#define DWT_CTRL_SLEEPEVTENA_Msk           (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos)         /*!< DWT CTRL: SLEEPEVTENA Mask */
+
+#define DWT_CTRL_EXCEVTENA_Pos             18                                          /*!< DWT CTRL: EXCEVTENA Position */
+#define DWT_CTRL_EXCEVTENA_Msk             (0x1UL << DWT_CTRL_EXCEVTENA_Pos)           /*!< DWT CTRL: EXCEVTENA Mask */
+
+#define DWT_CTRL_CPIEVTENA_Pos             17                                          /*!< DWT CTRL: CPIEVTENA Position */
+#define DWT_CTRL_CPIEVTENA_Msk             (0x1UL << DWT_CTRL_CPIEVTENA_Pos)           /*!< DWT CTRL: CPIEVTENA Mask */
+
+#define DWT_CTRL_EXCTRCENA_Pos             16                                          /*!< DWT CTRL: EXCTRCENA Position */
+#define DWT_CTRL_EXCTRCENA_Msk             (0x1UL << DWT_CTRL_EXCTRCENA_Pos)           /*!< DWT CTRL: EXCTRCENA Mask */
+
+#define DWT_CTRL_PCSAMPLENA_Pos            12                                          /*!< DWT CTRL: PCSAMPLENA Position */
+#define DWT_CTRL_PCSAMPLENA_Msk            (0x1UL << DWT_CTRL_PCSAMPLENA_Pos)          /*!< DWT CTRL: PCSAMPLENA Mask */
+
+#define DWT_CTRL_SYNCTAP_Pos               10                                          /*!< DWT CTRL: SYNCTAP Position */
+#define DWT_CTRL_SYNCTAP_Msk               (0x3UL << DWT_CTRL_SYNCTAP_Pos)             /*!< DWT CTRL: SYNCTAP Mask */
+
+#define DWT_CTRL_CYCTAP_Pos                 9                                          /*!< DWT CTRL: CYCTAP Position */
+#define DWT_CTRL_CYCTAP_Msk                (0x1UL << DWT_CTRL_CYCTAP_Pos)              /*!< DWT CTRL: CYCTAP Mask */
+
+#define DWT_CTRL_POSTINIT_Pos               5                                          /*!< DWT CTRL: POSTINIT Position */
+#define DWT_CTRL_POSTINIT_Msk              (0xFUL << DWT_CTRL_POSTINIT_Pos)            /*!< DWT CTRL: POSTINIT Mask */
+
+#define DWT_CTRL_POSTPRESET_Pos             1                                          /*!< DWT CTRL: POSTPRESET Position */
+#define DWT_CTRL_POSTPRESET_Msk            (0xFUL << DWT_CTRL_POSTPRESET_Pos)          /*!< DWT CTRL: POSTPRESET Mask */
+
+#define DWT_CTRL_CYCCNTENA_Pos              0                                          /*!< DWT CTRL: CYCCNTENA Position */
+#define DWT_CTRL_CYCCNTENA_Msk             (0x1UL << DWT_CTRL_CYCCNTENA_Pos)           /*!< DWT CTRL: CYCCNTENA Mask */
+
+/* DWT CPI Count Register Definitions */
+#define DWT_CPICNT_CPICNT_Pos               0                                          /*!< DWT CPICNT: CPICNT Position */
+#define DWT_CPICNT_CPICNT_Msk              (0xFFUL << DWT_CPICNT_CPICNT_Pos)           /*!< DWT CPICNT: CPICNT Mask */
+
+/* DWT Exception Overhead Count Register Definitions */
+#define DWT_EXCCNT_EXCCNT_Pos               0                                          /*!< DWT EXCCNT: EXCCNT Position */
+#define DWT_EXCCNT_EXCCNT_Msk              (0xFFUL << DWT_EXCCNT_EXCCNT_Pos)           /*!< DWT EXCCNT: EXCCNT Mask */
+
+/* DWT Sleep Count Register Definitions */
+#define DWT_SLEEPCNT_SLEEPCNT_Pos           0                                          /*!< DWT SLEEPCNT: SLEEPCNT Position */
+#define DWT_SLEEPCNT_SLEEPCNT_Msk          (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos)       /*!< DWT SLEEPCNT: SLEEPCNT Mask */
+
+/* DWT LSU Count Register Definitions */
+#define DWT_LSUCNT_LSUCNT_Pos               0                                          /*!< DWT LSUCNT: LSUCNT Position */
+#define DWT_LSUCNT_LSUCNT_Msk              (0xFFUL << DWT_LSUCNT_LSUCNT_Pos)           /*!< DWT LSUCNT: LSUCNT Mask */
+
+/* DWT Folded-instruction Count Register Definitions */
+#define DWT_FOLDCNT_FOLDCNT_Pos             0                                          /*!< DWT FOLDCNT: FOLDCNT Position */
+#define DWT_FOLDCNT_FOLDCNT_Msk            (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos)         /*!< DWT FOLDCNT: FOLDCNT Mask */
+
+/* DWT Comparator Mask Register Definitions */
+#define DWT_MASK_MASK_Pos                   0                                          /*!< DWT MASK: MASK Position */
+#define DWT_MASK_MASK_Msk                  (0x1FUL << DWT_MASK_MASK_Pos)               /*!< DWT MASK: MASK Mask */
+
+/* DWT Comparator Function Register Definitions */
+#define DWT_FUNCTION_MATCHED_Pos           24                                          /*!< DWT FUNCTION: MATCHED Position */
+#define DWT_FUNCTION_MATCHED_Msk           (0x1UL << DWT_FUNCTION_MATCHED_Pos)         /*!< DWT FUNCTION: MATCHED Mask */
+
+#define DWT_FUNCTION_DATAVADDR1_Pos        16                                          /*!< DWT FUNCTION: DATAVADDR1 Position */
+#define DWT_FUNCTION_DATAVADDR1_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos)      /*!< DWT FUNCTION: DATAVADDR1 Mask */
+
+#define DWT_FUNCTION_DATAVADDR0_Pos        12                                          /*!< DWT FUNCTION: DATAVADDR0 Position */
+#define DWT_FUNCTION_DATAVADDR0_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos)      /*!< DWT FUNCTION: DATAVADDR0 Mask */
+
+#define DWT_FUNCTION_DATAVSIZE_Pos         10                                          /*!< DWT FUNCTION: DATAVSIZE Position */
+#define DWT_FUNCTION_DATAVSIZE_Msk         (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos)       /*!< DWT FUNCTION: DATAVSIZE Mask */
+
+#define DWT_FUNCTION_LNK1ENA_Pos            9                                          /*!< DWT FUNCTION: LNK1ENA Position */
+#define DWT_FUNCTION_LNK1ENA_Msk           (0x1UL << DWT_FUNCTION_LNK1ENA_Pos)         /*!< DWT FUNCTION: LNK1ENA Mask */
+
+#define DWT_FUNCTION_DATAVMATCH_Pos         8                                          /*!< DWT FUNCTION: DATAVMATCH Position */
+#define DWT_FUNCTION_DATAVMATCH_Msk        (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos)      /*!< DWT FUNCTION: DATAVMATCH Mask */
+
+#define DWT_FUNCTION_CYCMATCH_Pos           7                                          /*!< DWT FUNCTION: CYCMATCH Position */
+#define DWT_FUNCTION_CYCMATCH_Msk          (0x1UL << DWT_FUNCTION_CYCMATCH_Pos)        /*!< DWT FUNCTION: CYCMATCH Mask */
+
+#define DWT_FUNCTION_EMITRANGE_Pos          5                                          /*!< DWT FUNCTION: EMITRANGE Position */
+#define DWT_FUNCTION_EMITRANGE_Msk         (0x1UL << DWT_FUNCTION_EMITRANGE_Pos)       /*!< DWT FUNCTION: EMITRANGE Mask */
+
+#define DWT_FUNCTION_FUNCTION_Pos           0                                          /*!< DWT FUNCTION: FUNCTION Position */
+#define DWT_FUNCTION_FUNCTION_Msk          (0xFUL << DWT_FUNCTION_FUNCTION_Pos)        /*!< DWT FUNCTION: FUNCTION Mask */
+
+/*@}*/ /* end of group CMSIS_DWT */
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_TPI     Trace Port Interface (TPI)
+    \brief      Type definitions for the Trace Port Interface (TPI)
+  @{
+ */
+
+/** \brief  Structure type to access the Trace Port Interface Register (TPI).
+ */
+typedef struct
+{
+  __IO uint32_t SSPSR;                   /*!< Offset: 0x000 (R/ )  Supported Parallel Port Size Register     */
+  __IO uint32_t CSPSR;                   /*!< Offset: 0x004 (R/W)  Current Parallel Port Size Register */
+       uint32_t RESERVED0[2];
+  __IO uint32_t ACPR;                    /*!< Offset: 0x010 (R/W)  Asynchronous Clock Prescaler Register */
+       uint32_t RESERVED1[55];
+  __IO uint32_t SPPR;                    /*!< Offset: 0x0F0 (R/W)  Selected Pin Protocol Register */
+       uint32_t RESERVED2[131];
+  __I  uint32_t FFSR;                    /*!< Offset: 0x300 (R/ )  Formatter and Flush Status Register */
+  __IO uint32_t FFCR;                    /*!< Offset: 0x304 (R/W)  Formatter and Flush Control Register */
+  __I  uint32_t FSCR;                    /*!< Offset: 0x308 (R/ )  Formatter Synchronization Counter Register */
+       uint32_t RESERVED3[759];
+  __I  uint32_t TRIGGER;                 /*!< Offset: 0xEE8 (R/ )  TRIGGER */
+  __I  uint32_t FIFO0;                   /*!< Offset: 0xEEC (R/ )  Integration ETM Data */
+  __I  uint32_t ITATBCTR2;               /*!< Offset: 0xEF0 (R/ )  ITATBCTR2 */
+       uint32_t RESERVED4[1];
+  __I  uint32_t ITATBCTR0;               /*!< Offset: 0xEF8 (R/ )  ITATBCTR0 */
+  __I  uint32_t FIFO1;                   /*!< Offset: 0xEFC (R/ )  Integration ITM Data */
+  __IO uint32_t ITCTRL;                  /*!< Offset: 0xF00 (R/W)  Integration Mode Control */
+       uint32_t RESERVED5[39];
+  __IO uint32_t CLAIMSET;                /*!< Offset: 0xFA0 (R/W)  Claim tag set */
+  __IO uint32_t CLAIMCLR;                /*!< Offset: 0xFA4 (R/W)  Claim tag clear */
+       uint32_t RESERVED7[8];
+  __I  uint32_t DEVID;                   /*!< Offset: 0xFC8 (R/ )  TPIU_DEVID */
+  __I  uint32_t DEVTYPE;                 /*!< Offset: 0xFCC (R/ )  TPIU_DEVTYPE */
+} TPI_Type;
+
+/* TPI Asynchronous Clock Prescaler Register Definitions */
+#define TPI_ACPR_PRESCALER_Pos              0                                          /*!< TPI ACPR: PRESCALER Position */
+#define TPI_ACPR_PRESCALER_Msk             (0x1FFFUL << TPI_ACPR_PRESCALER_Pos)        /*!< TPI ACPR: PRESCALER Mask */
+
+/* TPI Selected Pin Protocol Register Definitions */
+#define TPI_SPPR_TXMODE_Pos                 0                                          /*!< TPI SPPR: TXMODE Position */
+#define TPI_SPPR_TXMODE_Msk                (0x3UL << TPI_SPPR_TXMODE_Pos)              /*!< TPI SPPR: TXMODE Mask */
+
+/* TPI Formatter and Flush Status Register Definitions */
+#define TPI_FFSR_FtNonStop_Pos              3                                          /*!< TPI FFSR: FtNonStop Position */
+#define TPI_FFSR_FtNonStop_Msk             (0x1UL << TPI_FFSR_FtNonStop_Pos)           /*!< TPI FFSR: FtNonStop Mask */
+
+#define TPI_FFSR_TCPresent_Pos              2                                          /*!< TPI FFSR: TCPresent Position */
+#define TPI_FFSR_TCPresent_Msk             (0x1UL << TPI_FFSR_TCPresent_Pos)           /*!< TPI FFSR: TCPresent Mask */
+
+#define TPI_FFSR_FtStopped_Pos              1                                          /*!< TPI FFSR: FtStopped Position */
+#define TPI_FFSR_FtStopped_Msk             (0x1UL << TPI_FFSR_FtStopped_Pos)           /*!< TPI FFSR: FtStopped Mask */
+
+#define TPI_FFSR_FlInProg_Pos               0                                          /*!< TPI FFSR: FlInProg Position */
+#define TPI_FFSR_FlInProg_Msk              (0x1UL << TPI_FFSR_FlInProg_Pos)            /*!< TPI FFSR: FlInProg Mask */
+
+/* TPI Formatter and Flush Control Register Definitions */
+#define TPI_FFCR_TrigIn_Pos                 8                                          /*!< TPI FFCR: TrigIn Position */
+#define TPI_FFCR_TrigIn_Msk                (0x1UL << TPI_FFCR_TrigIn_Pos)              /*!< TPI FFCR: TrigIn Mask */
+
+#define TPI_FFCR_EnFCont_Pos                1                                          /*!< TPI FFCR: EnFCont Position */
+#define TPI_FFCR_EnFCont_Msk               (0x1UL << TPI_FFCR_EnFCont_Pos)             /*!< TPI FFCR: EnFCont Mask */
+
+/* TPI TRIGGER Register Definitions */
+#define TPI_TRIGGER_TRIGGER_Pos             0                                          /*!< TPI TRIGGER: TRIGGER Position */
+#define TPI_TRIGGER_TRIGGER_Msk            (0x1UL << TPI_TRIGGER_TRIGGER_Pos)          /*!< TPI TRIGGER: TRIGGER Mask */
+
+/* TPI Integration ETM Data Register Definitions (FIFO0) */
+#define TPI_FIFO0_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO0: ITM_ATVALID Position */
+#define TPI_FIFO0_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos)        /*!< TPI FIFO0: ITM_ATVALID Mask */
+
+#define TPI_FIFO0_ITM_bytecount_Pos        27                                          /*!< TPI FIFO0: ITM_bytecount Position */
+#define TPI_FIFO0_ITM_bytecount_Msk        (0x3UL << TPI_FIFO0_ITM_bytecount_Pos)      /*!< TPI FIFO0: ITM_bytecount Mask */
+
+#define TPI_FIFO0_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO0: ETM_ATVALID Position */
+#define TPI_FIFO0_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos)        /*!< TPI FIFO0: ETM_ATVALID Mask */
+
+#define TPI_FIFO0_ETM_bytecount_Pos        24                                          /*!< TPI FIFO0: ETM_bytecount Position */
+#define TPI_FIFO0_ETM_bytecount_Msk        (0x3UL << TPI_FIFO0_ETM_bytecount_Pos)      /*!< TPI FIFO0: ETM_bytecount Mask */
+
+#define TPI_FIFO0_ETM2_Pos                 16                                          /*!< TPI FIFO0: ETM2 Position */
+#define TPI_FIFO0_ETM2_Msk                 (0xFFUL << TPI_FIFO0_ETM2_Pos)              /*!< TPI FIFO0: ETM2 Mask */
+
+#define TPI_FIFO0_ETM1_Pos                  8                                          /*!< TPI FIFO0: ETM1 Position */
+#define TPI_FIFO0_ETM1_Msk                 (0xFFUL << TPI_FIFO0_ETM1_Pos)              /*!< TPI FIFO0: ETM1 Mask */
+
+#define TPI_FIFO0_ETM0_Pos                  0                                          /*!< TPI FIFO0: ETM0 Position */
+#define TPI_FIFO0_ETM0_Msk                 (0xFFUL << TPI_FIFO0_ETM0_Pos)              /*!< TPI FIFO0: ETM0 Mask */
+
+/* TPI ITATBCTR2 Register Definitions */
+#define TPI_ITATBCTR2_ATREADY_Pos           0                                          /*!< TPI ITATBCTR2: ATREADY Position */
+#define TPI_ITATBCTR2_ATREADY_Msk          (0x1UL << TPI_ITATBCTR2_ATREADY_Pos)        /*!< TPI ITATBCTR2: ATREADY Mask */
+
+/* TPI Integration ITM Data Register Definitions (FIFO1) */
+#define TPI_FIFO1_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO1: ITM_ATVALID Position */
+#define TPI_FIFO1_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos)        /*!< TPI FIFO1: ITM_ATVALID Mask */
+
+#define TPI_FIFO1_ITM_bytecount_Pos        27                                          /*!< TPI FIFO1: ITM_bytecount Position */
+#define TPI_FIFO1_ITM_bytecount_Msk        (0x3UL << TPI_FIFO1_ITM_bytecount_Pos)      /*!< TPI FIFO1: ITM_bytecount Mask */
+
+#define TPI_FIFO1_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO1: ETM_ATVALID Position */
+#define TPI_FIFO1_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos)        /*!< TPI FIFO1: ETM_ATVALID Mask */
+
+#define TPI_FIFO1_ETM_bytecount_Pos        24                                          /*!< TPI FIFO1: ETM_bytecount Position */
+#define TPI_FIFO1_ETM_bytecount_Msk        (0x3UL << TPI_FIFO1_ETM_bytecount_Pos)      /*!< TPI FIFO1: ETM_bytecount Mask */
+
+#define TPI_FIFO1_ITM2_Pos                 16                                          /*!< TPI FIFO1: ITM2 Position */
+#define TPI_FIFO1_ITM2_Msk                 (0xFFUL << TPI_FIFO1_ITM2_Pos)              /*!< TPI FIFO1: ITM2 Mask */
+
+#define TPI_FIFO1_ITM1_Pos                  8                                          /*!< TPI FIFO1: ITM1 Position */
+#define TPI_FIFO1_ITM1_Msk                 (0xFFUL << TPI_FIFO1_ITM1_Pos)              /*!< TPI FIFO1: ITM1 Mask */
+
+#define TPI_FIFO1_ITM0_Pos                  0                                          /*!< TPI FIFO1: ITM0 Position */
+#define TPI_FIFO1_ITM0_Msk                 (0xFFUL << TPI_FIFO1_ITM0_Pos)              /*!< TPI FIFO1: ITM0 Mask */
+
+/* TPI ITATBCTR0 Register Definitions */
+#define TPI_ITATBCTR0_ATREADY_Pos           0                                          /*!< TPI ITATBCTR0: ATREADY Position */
+#define TPI_ITATBCTR0_ATREADY_Msk          (0x1UL << TPI_ITATBCTR0_ATREADY_Pos)        /*!< TPI ITATBCTR0: ATREADY Mask */
+
+/* TPI Integration Mode Control Register Definitions */
+#define TPI_ITCTRL_Mode_Pos                 0                                          /*!< TPI ITCTRL: Mode Position */
+#define TPI_ITCTRL_Mode_Msk                (0x1UL << TPI_ITCTRL_Mode_Pos)              /*!< TPI ITCTRL: Mode Mask */
+
+/* TPI DEVID Register Definitions */
+#define TPI_DEVID_NRZVALID_Pos             11                                          /*!< TPI DEVID: NRZVALID Position */
+#define TPI_DEVID_NRZVALID_Msk             (0x1UL << TPI_DEVID_NRZVALID_Pos)           /*!< TPI DEVID: NRZVALID Mask */
+
+#define TPI_DEVID_MANCVALID_Pos            10                                          /*!< TPI DEVID: MANCVALID Position */
+#define TPI_DEVID_MANCVALID_Msk            (0x1UL << TPI_DEVID_MANCVALID_Pos)          /*!< TPI DEVID: MANCVALID Mask */
+
+#define TPI_DEVID_PTINVALID_Pos             9                                          /*!< TPI DEVID: PTINVALID Position */
+#define TPI_DEVID_PTINVALID_Msk            (0x1UL << TPI_DEVID_PTINVALID_Pos)          /*!< TPI DEVID: PTINVALID Mask */
+
+#define TPI_DEVID_MinBufSz_Pos              6                                          /*!< TPI DEVID: MinBufSz Position */
+#define TPI_DEVID_MinBufSz_Msk             (0x7UL << TPI_DEVID_MinBufSz_Pos)           /*!< TPI DEVID: MinBufSz Mask */
+
+#define TPI_DEVID_AsynClkIn_Pos             5                                          /*!< TPI DEVID: AsynClkIn Position */
+#define TPI_DEVID_AsynClkIn_Msk            (0x1UL << TPI_DEVID_AsynClkIn_Pos)          /*!< TPI DEVID: AsynClkIn Mask */
+
+#define TPI_DEVID_NrTraceInput_Pos          0                                          /*!< TPI DEVID: NrTraceInput Position */
+#define TPI_DEVID_NrTraceInput_Msk         (0x1FUL << TPI_DEVID_NrTraceInput_Pos)      /*!< TPI DEVID: NrTraceInput Mask */
+
+/* TPI DEVTYPE Register Definitions */
+#define TPI_DEVTYPE_SubType_Pos             0                                          /*!< TPI DEVTYPE: SubType Position */
+#define TPI_DEVTYPE_SubType_Msk            (0xFUL << TPI_DEVTYPE_SubType_Pos)          /*!< TPI DEVTYPE: SubType Mask */
+
+#define TPI_DEVTYPE_MajorType_Pos           4                                          /*!< TPI DEVTYPE: MajorType Position */
+#define TPI_DEVTYPE_MajorType_Msk          (0xFUL << TPI_DEVTYPE_MajorType_Pos)        /*!< TPI DEVTYPE: MajorType Mask */
+
+/*@}*/ /* end of group CMSIS_TPI */
+
+
+#if (__MPU_PRESENT == 1)
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_MPU     Memory Protection Unit (MPU)
+    \brief      Type definitions for the Memory Protection Unit (MPU)
+  @{
+ */
+
+/** \brief  Structure type to access the Memory Protection Unit (MPU).
+ */
+typedef struct
+{
+  __I  uint32_t TYPE;                    /*!< Offset: 0x000 (R/ )  MPU Type Register                              */
+  __IO uint32_t CTRL;                    /*!< Offset: 0x004 (R/W)  MPU Control Register                           */
+  __IO uint32_t RNR;                     /*!< Offset: 0x008 (R/W)  MPU Region RNRber Register                     */
+  __IO uint32_t RBAR;                    /*!< Offset: 0x00C (R/W)  MPU Region Base Address Register               */
+  __IO uint32_t RASR;                    /*!< Offset: 0x010 (R/W)  MPU Region Attribute and Size Register         */
+  __IO uint32_t RBAR_A1;                 /*!< Offset: 0x014 (R/W)  MPU Alias 1 Region Base Address Register       */
+  __IO uint32_t RASR_A1;                 /*!< Offset: 0x018 (R/W)  MPU Alias 1 Region Attribute and Size Register */
+  __IO uint32_t RBAR_A2;                 /*!< Offset: 0x01C (R/W)  MPU Alias 2 Region Base Address Register       */
+  __IO uint32_t RASR_A2;                 /*!< Offset: 0x020 (R/W)  MPU Alias 2 Region Attribute and Size Register */
+  __IO uint32_t RBAR_A3;                 /*!< Offset: 0x024 (R/W)  MPU Alias 3 Region Base Address Register       */
+  __IO uint32_t RASR_A3;                 /*!< Offset: 0x028 (R/W)  MPU Alias 3 Region Attribute and Size Register */
+} MPU_Type;
+
+/* MPU Type Register */
+#define MPU_TYPE_IREGION_Pos               16                                             /*!< MPU TYPE: IREGION Position */
+#define MPU_TYPE_IREGION_Msk               (0xFFUL << MPU_TYPE_IREGION_Pos)               /*!< MPU TYPE: IREGION Mask */
+
+#define MPU_TYPE_DREGION_Pos                8                                             /*!< MPU TYPE: DREGION Position */
+#define MPU_TYPE_DREGION_Msk               (0xFFUL << MPU_TYPE_DREGION_Pos)               /*!< MPU TYPE: DREGION Mask */
+
+#define MPU_TYPE_SEPARATE_Pos               0                                             /*!< MPU TYPE: SEPARATE Position */
+#define MPU_TYPE_SEPARATE_Msk              (1UL << MPU_TYPE_SEPARATE_Pos)                 /*!< MPU TYPE: SEPARATE Mask */
+
+/* MPU Control Register */
+#define MPU_CTRL_PRIVDEFENA_Pos             2                                             /*!< MPU CTRL: PRIVDEFENA Position */
+#define MPU_CTRL_PRIVDEFENA_Msk            (1UL << MPU_CTRL_PRIVDEFENA_Pos)               /*!< MPU CTRL: PRIVDEFENA Mask */
+
+#define MPU_CTRL_HFNMIENA_Pos               1                                             /*!< MPU CTRL: HFNMIENA Position */
+#define MPU_CTRL_HFNMIENA_Msk              (1UL << MPU_CTRL_HFNMIENA_Pos)                 /*!< MPU CTRL: HFNMIENA Mask */
+
+#define MPU_CTRL_ENABLE_Pos                 0                                             /*!< MPU CTRL: ENABLE Position */
+#define MPU_CTRL_ENABLE_Msk                (1UL << MPU_CTRL_ENABLE_Pos)                   /*!< MPU CTRL: ENABLE Mask */
+
+/* MPU Region Number Register */
+#define MPU_RNR_REGION_Pos                  0                                             /*!< MPU RNR: REGION Position */
+#define MPU_RNR_REGION_Msk                 (0xFFUL << MPU_RNR_REGION_Pos)                 /*!< MPU RNR: REGION Mask */
+
+/* MPU Region Base Address Register */
+#define MPU_RBAR_ADDR_Pos                   5                                             /*!< MPU RBAR: ADDR Position */
+#define MPU_RBAR_ADDR_Msk                  (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos)             /*!< MPU RBAR: ADDR Mask */
+
+#define MPU_RBAR_VALID_Pos                  4                                             /*!< MPU RBAR: VALID Position */
+#define MPU_RBAR_VALID_Msk                 (1UL << MPU_RBAR_VALID_Pos)                    /*!< MPU RBAR: VALID Mask */
+
+#define MPU_RBAR_REGION_Pos                 0                                             /*!< MPU RBAR: REGION Position */
+#define MPU_RBAR_REGION_Msk                (0xFUL << MPU_RBAR_REGION_Pos)                 /*!< MPU RBAR: REGION Mask */
+
+/* MPU Region Attribute and Size Register */
+#define MPU_RASR_ATTRS_Pos                 16                                             /*!< MPU RASR: MPU Region Attribute field Position */
+#define MPU_RASR_ATTRS_Msk                 (0xFFFFUL << MPU_RASR_ATTRS_Pos)               /*!< MPU RASR: MPU Region Attribute field Mask */
+
+#define MPU_RASR_XN_Pos                    28                                             /*!< MPU RASR: ATTRS.XN Position */
+#define MPU_RASR_XN_Msk                    (1UL << MPU_RASR_XN_Pos)                       /*!< MPU RASR: ATTRS.XN Mask */
+
+#define MPU_RASR_AP_Pos                    24                                             /*!< MPU RASR: ATTRS.AP Position */
+#define MPU_RASR_AP_Msk                    (0x7UL << MPU_RASR_AP_Pos)                     /*!< MPU RASR: ATTRS.AP Mask */
+
+#define MPU_RASR_TEX_Pos                   19                                             /*!< MPU RASR: ATTRS.TEX Position */
+#define MPU_RASR_TEX_Msk                   (0x7UL << MPU_RASR_TEX_Pos)                    /*!< MPU RASR: ATTRS.TEX Mask */
+
+#define MPU_RASR_S_Pos                     18                                             /*!< MPU RASR: ATTRS.S Position */
+#define MPU_RASR_S_Msk                     (1UL << MPU_RASR_S_Pos)                        /*!< MPU RASR: ATTRS.S Mask */
+
+#define MPU_RASR_C_Pos                     17                                             /*!< MPU RASR: ATTRS.C Position */
+#define MPU_RASR_C_Msk                     (1UL << MPU_RASR_C_Pos)                        /*!< MPU RASR: ATTRS.C Mask */
+
+#define MPU_RASR_B_Pos                     16                                             /*!< MPU RASR: ATTRS.B Position */
+#define MPU_RASR_B_Msk                     (1UL << MPU_RASR_B_Pos)                        /*!< MPU RASR: ATTRS.B Mask */
+
+#define MPU_RASR_SRD_Pos                    8                                             /*!< MPU RASR: Sub-Region Disable Position */
+#define MPU_RASR_SRD_Msk                   (0xFFUL << MPU_RASR_SRD_Pos)                   /*!< MPU RASR: Sub-Region Disable Mask */
+
+#define MPU_RASR_SIZE_Pos                   1                                             /*!< MPU RASR: Region Size Field Position */
+#define MPU_RASR_SIZE_Msk                  (0x1FUL << MPU_RASR_SIZE_Pos)                  /*!< MPU RASR: Region Size Field Mask */
+
+#define MPU_RASR_ENABLE_Pos                 0                                             /*!< MPU RASR: Region enable bit Position */
+#define MPU_RASR_ENABLE_Msk                (1UL << MPU_RASR_ENABLE_Pos)                   /*!< MPU RASR: Region enable bit Disable Mask */
+
+/*@} end of group CMSIS_MPU */
+#endif
+
+
+#if (__FPU_PRESENT == 1)
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_FPU     Floating Point Unit (FPU)
+    \brief      Type definitions for the Floating Point Unit (FPU)
+  @{
+ */
+
+/** \brief  Structure type to access the Floating Point Unit (FPU).
+ */
+typedef struct
+{
+       uint32_t RESERVED0[1];
+  __IO uint32_t FPCCR;                   /*!< Offset: 0x004 (R/W)  Floating-Point Context Control Register               */
+  __IO uint32_t FPCAR;                   /*!< Offset: 0x008 (R/W)  Floating-Point Context Address Register               */
+  __IO uint32_t FPDSCR;                  /*!< Offset: 0x00C (R/W)  Floating-Point Default Status Control Register        */
+  __I  uint32_t MVFR0;                   /*!< Offset: 0x010 (R/ )  Media and FP Feature Register 0                       */
+  __I  uint32_t MVFR1;                   /*!< Offset: 0x014 (R/ )  Media and FP Feature Register 1                       */
+} FPU_Type;
+
+/* Floating-Point Context Control Register */
+#define FPU_FPCCR_ASPEN_Pos                31                                             /*!< FPCCR: ASPEN bit Position */
+#define FPU_FPCCR_ASPEN_Msk                (1UL << FPU_FPCCR_ASPEN_Pos)                   /*!< FPCCR: ASPEN bit Mask */
+
+#define FPU_FPCCR_LSPEN_Pos                30                                             /*!< FPCCR: LSPEN Position */
+#define FPU_FPCCR_LSPEN_Msk                (1UL << FPU_FPCCR_LSPEN_Pos)                   /*!< FPCCR: LSPEN bit Mask */
+
+#define FPU_FPCCR_MONRDY_Pos                8                                             /*!< FPCCR: MONRDY Position */
+#define FPU_FPCCR_MONRDY_Msk               (1UL << FPU_FPCCR_MONRDY_Pos)                  /*!< FPCCR: MONRDY bit Mask */
+
+#define FPU_FPCCR_BFRDY_Pos                 6                                             /*!< FPCCR: BFRDY Position */
+#define FPU_FPCCR_BFRDY_Msk                (1UL << FPU_FPCCR_BFRDY_Pos)                   /*!< FPCCR: BFRDY bit Mask */
+
+#define FPU_FPCCR_MMRDY_Pos                 5                                             /*!< FPCCR: MMRDY Position */
+#define FPU_FPCCR_MMRDY_Msk                (1UL << FPU_FPCCR_MMRDY_Pos)                   /*!< FPCCR: MMRDY bit Mask */
+
+#define FPU_FPCCR_HFRDY_Pos                 4                                             /*!< FPCCR: HFRDY Position */
+#define FPU_FPCCR_HFRDY_Msk                (1UL << FPU_FPCCR_HFRDY_Pos)                   /*!< FPCCR: HFRDY bit Mask */
+
+#define FPU_FPCCR_THREAD_Pos                3                                             /*!< FPCCR: processor mode bit Position */
+#define FPU_FPCCR_THREAD_Msk               (1UL << FPU_FPCCR_THREAD_Pos)                  /*!< FPCCR: processor mode active bit Mask */
+
+#define FPU_FPCCR_USER_Pos                  1                                             /*!< FPCCR: privilege level bit Position */
+#define FPU_FPCCR_USER_Msk                 (1UL << FPU_FPCCR_USER_Pos)                    /*!< FPCCR: privilege level bit Mask */
+
+#define FPU_FPCCR_LSPACT_Pos                0                                             /*!< FPCCR: Lazy state preservation active bit Position */
+#define FPU_FPCCR_LSPACT_Msk               (1UL << FPU_FPCCR_LSPACT_Pos)                  /*!< FPCCR: Lazy state preservation active bit Mask */
+
+/* Floating-Point Context Address Register */
+#define FPU_FPCAR_ADDRESS_Pos               3                                             /*!< FPCAR: ADDRESS bit Position */
+#define FPU_FPCAR_ADDRESS_Msk              (0x1FFFFFFFUL << FPU_FPCAR_ADDRESS_Pos)        /*!< FPCAR: ADDRESS bit Mask */
+
+/* Floating-Point Default Status Control Register */
+#define FPU_FPDSCR_AHP_Pos                 26                                             /*!< FPDSCR: AHP bit Position */
+#define FPU_FPDSCR_AHP_Msk                 (1UL << FPU_FPDSCR_AHP_Pos)                    /*!< FPDSCR: AHP bit Mask */
+
+#define FPU_FPDSCR_DN_Pos                  25                                             /*!< FPDSCR: DN bit Position */
+#define FPU_FPDSCR_DN_Msk                  (1UL << FPU_FPDSCR_DN_Pos)                     /*!< FPDSCR: DN bit Mask */
+
+#define FPU_FPDSCR_FZ_Pos                  24                                             /*!< FPDSCR: FZ bit Position */
+#define FPU_FPDSCR_FZ_Msk                  (1UL << FPU_FPDSCR_FZ_Pos)                     /*!< FPDSCR: FZ bit Mask */
+
+#define FPU_FPDSCR_RMode_Pos               22                                             /*!< FPDSCR: RMode bit Position */
+#define FPU_FPDSCR_RMode_Msk               (3UL << FPU_FPDSCR_RMode_Pos)                  /*!< FPDSCR: RMode bit Mask */
+
+/* Media and FP Feature Register 0 */
+#define FPU_MVFR0_FP_rounding_modes_Pos    28                                             /*!< MVFR0: FP rounding modes bits Position */
+#define FPU_MVFR0_FP_rounding_modes_Msk    (0xFUL << FPU_MVFR0_FP_rounding_modes_Pos)     /*!< MVFR0: FP rounding modes bits Mask */
+
+#define FPU_MVFR0_Short_vectors_Pos        24                                             /*!< MVFR0: Short vectors bits Position */
+#define FPU_MVFR0_Short_vectors_Msk        (0xFUL << FPU_MVFR0_Short_vectors_Pos)         /*!< MVFR0: Short vectors bits Mask */
+
+#define FPU_MVFR0_Square_root_Pos          20                                             /*!< MVFR0: Square root bits Position */
+#define FPU_MVFR0_Square_root_Msk          (0xFUL << FPU_MVFR0_Square_root_Pos)           /*!< MVFR0: Square root bits Mask */
+
+#define FPU_MVFR0_Divide_Pos               16                                             /*!< MVFR0: Divide bits Position */
+#define FPU_MVFR0_Divide_Msk               (0xFUL << FPU_MVFR0_Divide_Pos)                /*!< MVFR0: Divide bits Mask */
+
+#define FPU_MVFR0_FP_excep_trapping_Pos    12                                             /*!< MVFR0: FP exception trapping bits Position */
+#define FPU_MVFR0_FP_excep_trapping_Msk    (0xFUL << FPU_MVFR0_FP_excep_trapping_Pos)     /*!< MVFR0: FP exception trapping bits Mask */
+
+#define FPU_MVFR0_Double_precision_Pos      8                                             /*!< MVFR0: Double-precision bits Position */
+#define FPU_MVFR0_Double_precision_Msk     (0xFUL << FPU_MVFR0_Double_precision_Pos)      /*!< MVFR0: Double-precision bits Mask */
+
+#define FPU_MVFR0_Single_precision_Pos      4                                             /*!< MVFR0: Single-precision bits Position */
+#define FPU_MVFR0_Single_precision_Msk     (0xFUL << FPU_MVFR0_Single_precision_Pos)      /*!< MVFR0: Single-precision bits Mask */
+
+#define FPU_MVFR0_A_SIMD_registers_Pos      0                                             /*!< MVFR0: A_SIMD registers bits Position */
+#define FPU_MVFR0_A_SIMD_registers_Msk     (0xFUL << FPU_MVFR0_A_SIMD_registers_Pos)      /*!< MVFR0: A_SIMD registers bits Mask */
+
+/* Media and FP Feature Register 1 */
+#define FPU_MVFR1_FP_fused_MAC_Pos         28                                             /*!< MVFR1: FP fused MAC bits Position */
+#define FPU_MVFR1_FP_fused_MAC_Msk         (0xFUL << FPU_MVFR1_FP_fused_MAC_Pos)          /*!< MVFR1: FP fused MAC bits Mask */
+
+#define FPU_MVFR1_FP_HPFP_Pos              24                                             /*!< MVFR1: FP HPFP bits Position */
+#define FPU_MVFR1_FP_HPFP_Msk              (0xFUL << FPU_MVFR1_FP_HPFP_Pos)               /*!< MVFR1: FP HPFP bits Mask */
+
+#define FPU_MVFR1_D_NaN_mode_Pos            4                                             /*!< MVFR1: D_NaN mode bits Position */
+#define FPU_MVFR1_D_NaN_mode_Msk           (0xFUL << FPU_MVFR1_D_NaN_mode_Pos)            /*!< MVFR1: D_NaN mode bits Mask */
+
+#define FPU_MVFR1_FtZ_mode_Pos              0                                             /*!< MVFR1: FtZ mode bits Position */
+#define FPU_MVFR1_FtZ_mode_Msk             (0xFUL << FPU_MVFR1_FtZ_mode_Pos)              /*!< MVFR1: FtZ mode bits Mask */
+
+/*@} end of group CMSIS_FPU */
+#endif
+
+
+/** \ingroup  CMSIS_core_register
+    \defgroup CMSIS_CoreDebug       Core Debug Registers (CoreDebug)
+    \brief      Type definitions for the Core Debug Registers
+  @{
+ */
+
+/** \brief  Structure type to access the Core Debug Register (CoreDebug).
+ */
+typedef struct
+{
+  __IO uint32_t DHCSR;                   /*!< Offset: 0x000 (R/W)  Debug Halting Control and Status Register    */
+  __O  uint32_t DCRSR;                   /*!< Offset: 0x004 ( /W)  Debug Core Register Selector Register        */
+  __IO uint32_t DCRDR;                   /*!< Offset: 0x008 (R/W)  Debug Core Register Data Register            */
+  __IO uint32_t DEMCR;                   /*!< Offset: 0x00C (R/W)  Debug Exception and Monitor Control Register */
+} CoreDebug_Type;
+
+/* Debug Halting Control and Status Register */
+#define CoreDebug_DHCSR_DBGKEY_Pos         16                                             /*!< CoreDebug DHCSR: DBGKEY Position */
+#define CoreDebug_DHCSR_DBGKEY_Msk         (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos)       /*!< CoreDebug DHCSR: DBGKEY Mask */
+
+#define CoreDebug_DHCSR_S_RESET_ST_Pos     25                                             /*!< CoreDebug DHCSR: S_RESET_ST Position */
+#define CoreDebug_DHCSR_S_RESET_ST_Msk     (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos)        /*!< CoreDebug DHCSR: S_RESET_ST Mask */
+
+#define CoreDebug_DHCSR_S_RETIRE_ST_Pos    24                                             /*!< CoreDebug DHCSR: S_RETIRE_ST Position */
+#define CoreDebug_DHCSR_S_RETIRE_ST_Msk    (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos)       /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */
+
+#define CoreDebug_DHCSR_S_LOCKUP_Pos       19                                             /*!< CoreDebug DHCSR: S_LOCKUP Position */
+#define CoreDebug_DHCSR_S_LOCKUP_Msk       (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos)          /*!< CoreDebug DHCSR: S_LOCKUP Mask */
+
+#define CoreDebug_DHCSR_S_SLEEP_Pos        18                                             /*!< CoreDebug DHCSR: S_SLEEP Position */
+#define CoreDebug_DHCSR_S_SLEEP_Msk        (1UL << CoreDebug_DHCSR_S_SLEEP_Pos)           /*!< CoreDebug DHCSR: S_SLEEP Mask */
+
+#define CoreDebug_DHCSR_S_HALT_Pos         17                                             /*!< CoreDebug DHCSR: S_HALT Position */
+#define CoreDebug_DHCSR_S_HALT_Msk         (1UL << CoreDebug_DHCSR_S_HALT_Pos)            /*!< CoreDebug DHCSR: S_HALT Mask */
+
+#define CoreDebug_DHCSR_S_REGRDY_Pos       16                                             /*!< CoreDebug DHCSR: S_REGRDY Position */
+#define CoreDebug_DHCSR_S_REGRDY_Msk       (1UL << CoreDebug_DHCSR_S_REGRDY_Pos)          /*!< CoreDebug DHCSR: S_REGRDY Mask */
+
+#define CoreDebug_DHCSR_C_SNAPSTALL_Pos     5                                             /*!< CoreDebug DHCSR: C_SNAPSTALL Position */
+#define CoreDebug_DHCSR_C_SNAPSTALL_Msk    (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos)       /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */
+
+#define CoreDebug_DHCSR_C_MASKINTS_Pos      3                                             /*!< CoreDebug DHCSR: C_MASKINTS Position */
+#define CoreDebug_DHCSR_C_MASKINTS_Msk     (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos)        /*!< CoreDebug DHCSR: C_MASKINTS Mask */
+
+#define CoreDebug_DHCSR_C_STEP_Pos          2                                             /*!< CoreDebug DHCSR: C_STEP Position */
+#define CoreDebug_DHCSR_C_STEP_Msk         (1UL << CoreDebug_DHCSR_C_STEP_Pos)            /*!< CoreDebug DHCSR: C_STEP Mask */
+
+#define CoreDebug_DHCSR_C_HALT_Pos          1                                             /*!< CoreDebug DHCSR: C_HALT Position */
+#define CoreDebug_DHCSR_C_HALT_Msk         (1UL << CoreDebug_DHCSR_C_HALT_Pos)            /*!< CoreDebug DHCSR: C_HALT Mask */
+
+#define CoreDebug_DHCSR_C_DEBUGEN_Pos       0                                             /*!< CoreDebug DHCSR: C_DEBUGEN Position */
+#define CoreDebug_DHCSR_C_DEBUGEN_Msk      (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos)         /*!< CoreDebug DHCSR: C_DEBUGEN Mask */
+
+/* Debug Core Register Selector Register */
+#define CoreDebug_DCRSR_REGWnR_Pos         16                                             /*!< CoreDebug DCRSR: REGWnR Position */
+#define CoreDebug_DCRSR_REGWnR_Msk         (1UL << CoreDebug_DCRSR_REGWnR_Pos)            /*!< CoreDebug DCRSR: REGWnR Mask */
+
+#define CoreDebug_DCRSR_REGSEL_Pos          0                                             /*!< CoreDebug DCRSR: REGSEL Position */
+#define CoreDebug_DCRSR_REGSEL_Msk         (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos)         /*!< CoreDebug DCRSR: REGSEL Mask */
+
+/* Debug Exception and Monitor Control Register */
+#define CoreDebug_DEMCR_TRCENA_Pos         24                                             /*!< CoreDebug DEMCR: TRCENA Position */
+#define CoreDebug_DEMCR_TRCENA_Msk         (1UL << CoreDebug_DEMCR_TRCENA_Pos)            /*!< CoreDebug DEMCR: TRCENA Mask */
+
+#define CoreDebug_DEMCR_MON_REQ_Pos        19                                             /*!< CoreDebug DEMCR: MON_REQ Position */
+#define CoreDebug_DEMCR_MON_REQ_Msk        (1UL << CoreDebug_DEMCR_MON_REQ_Pos)           /*!< CoreDebug DEMCR: MON_REQ Mask */
+
+#define CoreDebug_DEMCR_MON_STEP_Pos       18                                             /*!< CoreDebug DEMCR: MON_STEP Position */
+#define CoreDebug_DEMCR_MON_STEP_Msk       (1UL << CoreDebug_DEMCR_MON_STEP_Pos)          /*!< CoreDebug DEMCR: MON_STEP Mask */
+
+#define CoreDebug_DEMCR_MON_PEND_Pos       17                                             /*!< CoreDebug DEMCR: MON_PEND Position */
+#define CoreDebug_DEMCR_MON_PEND_Msk       (1UL << CoreDebug_DEMCR_MON_PEND_Pos)          /*!< CoreDebug DEMCR: MON_PEND Mask */
+
+#define CoreDebug_DEMCR_MON_EN_Pos         16                                             /*!< CoreDebug DEMCR: MON_EN Position */
+#define CoreDebug_DEMCR_MON_EN_Msk         (1UL << CoreDebug_DEMCR_MON_EN_Pos)            /*!< CoreDebug DEMCR: MON_EN Mask */
+
+#define CoreDebug_DEMCR_VC_HARDERR_Pos     10                                             /*!< CoreDebug DEMCR: VC_HARDERR Position */
+#define CoreDebug_DEMCR_VC_HARDERR_Msk     (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos)        /*!< CoreDebug DEMCR: VC_HARDERR Mask */
+
+#define CoreDebug_DEMCR_VC_INTERR_Pos       9                                             /*!< CoreDebug DEMCR: VC_INTERR Position */
+#define CoreDebug_DEMCR_VC_INTERR_Msk      (1UL << CoreDebug_DEMCR_VC_INTERR_Pos)         /*!< CoreDebug DEMCR: VC_INTERR Mask */
+
+#define CoreDebug_DEMCR_VC_BUSERR_Pos       8                                             /*!< CoreDebug DEMCR: VC_BUSERR Position */
+#define CoreDebug_DEMCR_VC_BUSERR_Msk      (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos)         /*!< CoreDebug DEMCR: VC_BUSERR Mask */
+
+#define CoreDebug_DEMCR_VC_STATERR_Pos      7                                             /*!< CoreDebug DEMCR: VC_STATERR Position */
+#define CoreDebug_DEMCR_VC_STATERR_Msk     (1UL << CoreDebug_DEMCR_VC_STATERR_Pos)        /*!< CoreDebug DEMCR: VC_STATERR Mask */
+
+#define CoreDebug_DEMCR_VC_CHKERR_Pos       6                                             /*!< CoreDebug DEMCR: VC_CHKERR Position */
+#define CoreDebug_DEMCR_VC_CHKERR_Msk      (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos)         /*!< CoreDebug DEMCR: VC_CHKERR Mask */
+
+#define CoreDebug_DEMCR_VC_NOCPERR_Pos      5                                             /*!< CoreDebug DEMCR: VC_NOCPERR Position */
+#define CoreDebug_DEMCR_VC_NOCPERR_Msk     (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos)        /*!< CoreDebug DEMCR: VC_NOCPERR Mask */
+
+#define CoreDebug_DEMCR_VC_MMERR_Pos        4                                             /*!< CoreDebug DEMCR: VC_MMERR Position */
+#define CoreDebug_DEMCR_VC_MMERR_Msk       (1UL << CoreDebug_DEMCR_VC_MMERR_Pos)          /*!< CoreDebug DEMCR: VC_MMERR Mask */
+
+#define CoreDebug_DEMCR_VC_CORERESET_Pos    0                                             /*!< CoreDebug DEMCR: VC_CORERESET Position */
+#define CoreDebug_DEMCR_VC_CORERESET_Msk   (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos)      /*!< CoreDebug DEMCR: VC_CORERESET Mask */
+
+/*@} end of group CMSIS_CoreDebug */
+
+
+/** \ingroup    CMSIS_core_register
+    \defgroup   CMSIS_core_base     Core Definitions
+    \brief      Definitions for base addresses, unions, and structures.
+  @{
+ */
+
+/* Memory mapping of Cortex-M4 Hardware */
+#define SCS_BASE            (0xE000E000UL)                            /*!< System Control Space Base Address  */
+#define ITM_BASE            (0xE0000000UL)                            /*!< ITM Base Address                   */
+#define DWT_BASE            (0xE0001000UL)                            /*!< DWT Base Address                   */
+#define TPI_BASE            (0xE0040000UL)                            /*!< TPI Base Address                   */
+#define CoreDebug_BASE      (0xE000EDF0UL)                            /*!< Core Debug Base Address            */
+#define SysTick_BASE        (SCS_BASE +  0x0010UL)                    /*!< SysTick Base Address               */
+#define NVIC_BASE           (SCS_BASE +  0x0100UL)                    /*!< NVIC Base Address                  */
+#define SCB_BASE            (SCS_BASE +  0x0D00UL)                    /*!< System Control Block Base Address  */
+
+#define SCnSCB              ((SCnSCB_Type    *)     SCS_BASE      )   /*!< System control Register not in SCB */
+#define SCB                 ((SCB_Type       *)     SCB_BASE      )   /*!< SCB configuration struct           */
+#define SysTick             ((SysTick_Type   *)     SysTick_BASE  )   /*!< SysTick configuration struct       */
+#define NVIC                ((NVIC_Type      *)     NVIC_BASE     )   /*!< NVIC configuration struct          */
+#define ITM                 ((ITM_Type       *)     ITM_BASE      )   /*!< ITM configuration struct           */
+#define DWT                 ((DWT_Type       *)     DWT_BASE      )   /*!< DWT configuration struct           */
+#define TPI                 ((TPI_Type       *)     TPI_BASE      )   /*!< TPI configuration struct           */
+#define CoreDebug           ((CoreDebug_Type *)     CoreDebug_BASE)   /*!< Core Debug configuration struct    */
+
+#if (__MPU_PRESENT == 1)
+  #define MPU_BASE          (SCS_BASE +  0x0D90UL)                    /*!< Memory Protection Unit             */
+  #define MPU               ((MPU_Type       *)     MPU_BASE      )   /*!< Memory Protection Unit             */
+#endif
+
+#if (__FPU_PRESENT == 1)
+  #define FPU_BASE          (SCS_BASE +  0x0F30UL)                    /*!< Floating Point Unit                */
+  #define FPU               ((FPU_Type       *)     FPU_BASE      )   /*!< Floating Point Unit                */
+#endif
+
+/*@} */
+
+
+
+/*******************************************************************************
+ *                Hardware Abstraction Layer
+  Core Function Interface contains:
+  - Core NVIC Functions
+  - Core SysTick Functions
+  - Core Debug Functions
+  - Core Register Access Functions
+ ******************************************************************************/
+/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
+*/
+
+
+
+/* ##########################   NVIC functions  #################################### */
+/** \ingroup  CMSIS_Core_FunctionInterface
+    \defgroup CMSIS_Core_NVICFunctions NVIC Functions
+    \brief      Functions that manage interrupts and exceptions via the NVIC.
+    @{
+ */
+
+/** \brief  Set Priority Grouping
+
+  The function sets the priority grouping field using the required unlock sequence.
+  The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.
+  Only values from 0..7 are used.
+  In case of a conflict between priority grouping and available
+  priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
+
+    \param [in]      PriorityGroup  Priority grouping field.
+ */
+__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
+{
+  uint32_t reg_value;
+  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07);               /* only values 0..7 are used          */
+
+  reg_value  =  SCB->AIRCR;                                                   /* read old register configuration    */
+  reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk);             /* clear bits to change               */
+  reg_value  =  (reg_value                                 |
+                ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |
+                (PriorityGroupTmp << 8));                                     /* Insert write key and priorty group */
+  SCB->AIRCR =  reg_value;
+}
+
+
+/** \brief  Get Priority Grouping
+
+  The function reads the priority grouping field from the NVIC Interrupt Controller.
+
+    \return                Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).
+ */
+__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
+{
+  return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos);   /* read priority grouping field */
+}
+
+
+/** \brief  Enable External Interrupt
+
+    The function enables a device-specific interrupt in the NVIC interrupt controller.
+
+    \param [in]      IRQn  External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
+{
+/*  NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F));  enable interrupt */
+  NVIC->ISER[(uint32_t)((int32_t)IRQn) >> 5] = (uint32_t)(1 << ((uint32_t)((int32_t)IRQn) & (uint32_t)0x1F)); /* enable interrupt */
+}
+
+
+/** \brief  Disable External Interrupt
+
+    The function disables a device-specific interrupt in the NVIC interrupt controller.
+
+    \param [in]      IRQn  External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
+{
+  NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */
+}
+
+
+/** \brief  Get Pending Interrupt
+
+    The function reads the pending register in the NVIC and returns the pending bit
+    for the specified interrupt.
+
+    \param [in]      IRQn  Interrupt number.
+
+    \return             0  Interrupt status is not pending.
+    \return             1  Interrupt status is pending.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
+{
+  return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */
+}
+
+
+/** \brief  Set Pending Interrupt
+
+    The function sets the pending bit of an external interrupt.
+
+    \param [in]      IRQn  Interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
+{
+  NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */
+}
+
+
+/** \brief  Clear Pending Interrupt
+
+    The function clears the pending bit of an external interrupt.
+
+    \param [in]      IRQn  External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
+{
+  NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
+}
+
+
+/** \brief  Get Active Interrupt
+
+    The function reads the active register in NVIC and returns the active bit.
+
+    \param [in]      IRQn  Interrupt number.
+
+    \return             0  Interrupt status is not active.
+    \return             1  Interrupt status is active.
+ */
+__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
+{
+  return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */
+}
+
+
+/** \brief  Set Interrupt Priority
+
+    The function sets the priority of an interrupt.
+
+    \note The priority cannot be set for every core interrupt.
+
+    \param [in]      IRQn  Interrupt number.
+    \param [in]  priority  Priority to set.
+ */
+__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
+{
+  if(IRQn < 0) {
+    SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M  System Interrupts */
+  else {
+    NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff);    }        /* set Priority for device specific Interrupts  */
+}
+
+
+/** \brief  Get Interrupt Priority
+
+    The function reads the priority of an interrupt. The interrupt
+    number can be positive to specify an external (device specific)
+    interrupt, or negative to specify an internal (core) interrupt.
+
+
+    \param [in]   IRQn  Interrupt number.
+    \return             Interrupt Priority. Value is aligned automatically to the implemented
+                        priority bits of the microcontroller.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
+{
+
+  if(IRQn < 0) {
+    return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for Cortex-M  system interrupts */
+  else {
+    return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)]           >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for device specific interrupts  */
+}
+
+
+/** \brief  Encode Priority
+
+    The function encodes the priority for an interrupt with the given priority group,
+    preemptive priority value, and subpriority value.
+    In case of a conflict between priority grouping and available
+    priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.
+
+    \param [in]     PriorityGroup  Used priority group.
+    \param [in]   PreemptPriority  Preemptive priority value (starting from 0).
+    \param [in]       SubPriority  Subpriority value (starting from 0).
+    \return                        Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
+ */
+__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
+{
+  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */
+  uint32_t PreemptPriorityBits;
+  uint32_t SubPriorityBits;
+
+  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
+  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
+
+  return (
+           ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |
+           ((SubPriority     & ((1 << (SubPriorityBits    )) - 1)))
+         );
+}
+
+
+/** \brief  Decode Priority
+
+    The function decodes an interrupt priority value with a given priority group to
+    preemptive priority value and subpriority value.
+    In case of a conflict between priority grouping and available
+    priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.
+
+    \param [in]         Priority   Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
+    \param [in]     PriorityGroup  Used priority group.
+    \param [out] pPreemptPriority  Preemptive priority value (starting from 0).
+    \param [out]     pSubPriority  Subpriority value (starting from 0).
+ */
+__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)
+{
+  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */
+  uint32_t PreemptPriorityBits;
+  uint32_t SubPriorityBits;
+
+  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
+  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
+
+  *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);
+  *pSubPriority     = (Priority                   ) & ((1 << (SubPriorityBits    )) - 1);
+}
+
+
+/** \brief  System Reset
+
+    The function initiates a system reset request to reset the MCU.
+ */
+__STATIC_INLINE void NVIC_SystemReset(void)
+{
+  __DSB();                                                     /* Ensure all outstanding memory accesses included
+                                                                  buffered write are completed before reset */
+  SCB->AIRCR  = ((0x5FA << SCB_AIRCR_VECTKEY_Pos)      |
+                 (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
+                 SCB_AIRCR_SYSRESETREQ_Msk);                   /* Keep priority group unchanged */
+  __DSB();                                                     /* Ensure completion of memory access */
+  while(1);                                                    /* wait until reset */
+}
+
+/*@} end of CMSIS_Core_NVICFunctions */
+
+
+
+/* ##################################    SysTick function  ############################################ */
+/** \ingroup  CMSIS_Core_FunctionInterface
+    \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
+    \brief      Functions that configure the System.
+  @{
+ */
+
+#if (__Vendor_SysTickConfig == 0)
+
+/** \brief  System Tick Configuration
+
+    The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
+    Counter is in free running mode to generate periodic interrupts.
+
+    \param [in]  ticks  Number of ticks between two interrupts.
+
+    \return          0  Function succeeded.
+    \return          1  Function failed.
+
+    \note     When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
+    function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
+    must contain a vendor-specific implementation of this function.
+
+ */
+__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
+{
+  if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk)  return (1);      /* Reload value impossible */
+
+  SysTick->LOAD  = ticks - 1;                                  /* set reload register */
+  NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1);  /* set Priority for Systick Interrupt */
+  SysTick->VAL   = 0;                                          /* Load the SysTick Counter Value */
+  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |
+                   SysTick_CTRL_TICKINT_Msk   |
+                   SysTick_CTRL_ENABLE_Msk;                    /* Enable SysTick IRQ and SysTick Timer */
+  return (0);                                                  /* Function successful */
+}
+
+#endif
+
+/*@} end of CMSIS_Core_SysTickFunctions */
+
+
+
+/* ##################################### Debug In/Output function ########################################### */
+/** \ingroup  CMSIS_Core_FunctionInterface
+    \defgroup CMSIS_core_DebugFunctions ITM Functions
+    \brief   Functions that access the ITM debug interface.
+  @{
+ */
+
+extern volatile int32_t ITM_RxBuffer;                    /*!< External variable to receive characters.                         */
+#define                 ITM_RXBUFFER_EMPTY    0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */
+
+
+/** \brief  ITM Send Character
+
+    The function transmits a character via the ITM channel 0, and
+    \li Just returns when no debugger is connected that has booked the output.
+    \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.
+
+    \param [in]     ch  Character to transmit.
+
+    \returns            Character to transmit.
+ */
+__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
+{
+  if ((ITM->TCR & ITM_TCR_ITMENA_Msk)                  &&      /* ITM enabled */
+      (ITM->TER & (1UL << 0)        )                    )     /* ITM Port #0 enabled */
+  {
+    while (ITM->PORT[0].u32 == 0);
+    ITM->PORT[0].u8 = (uint8_t) ch;
+  }
+  return (ch);
+}
+
+
+/** \brief  ITM Receive Character
+
+    The function inputs a character via the external variable \ref ITM_RxBuffer.
+
+    \return             Received character.
+    \return         -1  No character pending.
+ */
+__STATIC_INLINE int32_t ITM_ReceiveChar (void) {
+  int32_t ch = -1;                           /* no character available */
+
+  if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {
+    ch = ITM_RxBuffer;
+    ITM_RxBuffer = ITM_RXBUFFER_EMPTY;       /* ready for next character */
+  }
+
+  return (ch);
+}
+
+
+/** \brief  ITM Check Character
+
+    The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.
+
+    \return          0  No character available.
+    \return          1  Character available.
+ */
+__STATIC_INLINE int32_t ITM_CheckChar (void) {
+
+  if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {
+    return (0);                                 /* no character available */
+  } else {
+    return (1);                                 /*    character available */
+  }
+}
+
+/*@} end of CMSIS_core_DebugFunctions */
+
+#endif /* __CORE_CM4_H_DEPENDANT */
+
+#endif /* __CMSIS_GENERIC */
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
new file mode 100644
index 000000000..af1831ee1
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
@@ -0,0 +1,673 @@
+/**************************************************************************//**
+ * @file     core_cm4_simd.h
+ * @brief    CMSIS Cortex-M4 SIMD Header File
+ * @version  V3.20
+ * @date     25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+   All rights reserved.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+   - Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+   ---------------------------------------------------------------------------*/
+
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#ifndef __CORE_CM4_SIMD_H
+#define __CORE_CM4_SIMD_H
+
+
+/*******************************************************************************
+ *                Hardware Abstraction Layer
+ ******************************************************************************/
+
+
+/* ###################  Compiler specific Intrinsics  ########################### */
+/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
+  Access to dedicated SIMD instructions
+  @{
+*/
+
+#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+#define __SADD8                           __sadd8
+#define __QADD8                           __qadd8
+#define __SHADD8                          __shadd8
+#define __UADD8                           __uadd8
+#define __UQADD8                          __uqadd8
+#define __UHADD8                          __uhadd8
+#define __SSUB8                           __ssub8
+#define __QSUB8                           __qsub8
+#define __SHSUB8                          __shsub8
+#define __USUB8                           __usub8
+#define __UQSUB8                          __uqsub8
+#define __UHSUB8                          __uhsub8
+#define __SADD16                          __sadd16
+#define __QADD16                          __qadd16
+#define __SHADD16                         __shadd16
+#define __UADD16                          __uadd16
+#define __UQADD16                         __uqadd16
+#define __UHADD16                         __uhadd16
+#define __SSUB16                          __ssub16
+#define __QSUB16                          __qsub16
+#define __SHSUB16                         __shsub16
+#define __USUB16                          __usub16
+#define __UQSUB16                         __uqsub16
+#define __UHSUB16                         __uhsub16
+#define __SASX                            __sasx
+#define __QASX                            __qasx
+#define __SHASX                           __shasx
+#define __UASX                            __uasx
+#define __UQASX                           __uqasx
+#define __UHASX                           __uhasx
+#define __SSAX                            __ssax
+#define __QSAX                            __qsax
+#define __SHSAX                           __shsax
+#define __USAX                            __usax
+#define __UQSAX                           __uqsax
+#define __UHSAX                           __uhsax
+#define __USAD8                           __usad8
+#define __USADA8                          __usada8
+#define __SSAT16                          __ssat16
+#define __USAT16                          __usat16
+#define __UXTB16                          __uxtb16
+#define __UXTAB16                         __uxtab16
+#define __SXTB16                          __sxtb16
+#define __SXTAB16                         __sxtab16
+#define __SMUAD                           __smuad
+#define __SMUADX                          __smuadx
+#define __SMLAD                           __smlad
+#define __SMLADX                          __smladx
+#define __SMLALD                          __smlald
+#define __SMLALDX                         __smlaldx
+#define __SMUSD                           __smusd
+#define __SMUSDX                          __smusdx
+#define __SMLSD                           __smlsd
+#define __SMLSDX                          __smlsdx
+#define __SMLSLD                          __smlsld
+#define __SMLSLDX                         __smlsldx
+#define __SEL                             __sel
+#define __QADD                            __qadd
+#define __QSUB                            __qsub
+
+#define __PKHBT(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0x0000FFFFUL) |  \
+                                           ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL)  )
+
+#define __PKHTB(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0xFFFF0000UL) |  \
+                                           ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL)  )
+
+#define __SMMLA(ARG1,ARG2,ARG3)          ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \
+                                                      ((int64_t)(ARG3) << 32)      ) >> 32))
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+#include <cmsis_iar.h>
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+#include <cmsis_ccs.h>
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+#define __SSAT16(ARG1,ARG2) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("ssat16 %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+#define __USAT16(ARG1,ARG2) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("usat16 %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTB16(uint32_t op1)
+{
+  uint32_t result;
+
+  __ASM volatile ("uxtb16 %0, %1" : "=r" (result) : "r" (op1));
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTB16(uint32_t op1)
+{
+  uint32_t result;
+
+  __ASM volatile ("sxtb16 %0, %1" : "=r" (result) : "r" (op1));
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUAD  (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+#define __SMLALD(ARG1,ARG2,ARG3) \
+({ \
+  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
+  __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+#define __SMLALDX(ARG1,ARG2,ARG3) \
+({ \
+  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
+  __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSD  (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+#define __SMLSLD(ARG1,ARG2,ARG3) \
+({ \
+  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
+  __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+#define __SMLSLDX(ARG1,ARG2,ARG3) \
+({ \
+  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
+  __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SEL  (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+#define __PKHBT(ARG1,ARG2,ARG3) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
+  __ASM ("pkhbt %0, %1, %2, lsl %3" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2), "I" (ARG3)  ); \
+  __RES; \
+ })
+
+#define __PKHTB(ARG1,ARG2,ARG3) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
+  if (ARG3 == 0) \
+    __ASM ("pkhtb %0, %1, %2" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2)  ); \
+  else \
+    __ASM ("pkhtb %0, %1, %2, asr %3" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2), "I" (ARG3)  ); \
+  __RES; \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3)
+{
+ int32_t result;
+
+ __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r"  (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+/* not yet supported */
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+#endif
+
+/*@} end of group CMSIS_SIMD_intrinsics */
+
+
+#endif /* __CORE_CM4_SIMD_H */
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/lib/mathlib/CMSIS/Include/core_cmFunc.h b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h
new file mode 100644
index 000000000..139bc3c5e
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h
@@ -0,0 +1,636 @@
+/**************************************************************************//**
+ * @file     core_cmFunc.h
+ * @brief    CMSIS Cortex-M Core Function Access Header File
+ * @version  V3.20
+ * @date     25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+   All rights reserved.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+   - Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+   POSSIBILITY OF SUCH DAMAGE.
+   ---------------------------------------------------------------------------*/
+
+
+#ifndef __CORE_CMFUNC_H
+#define __CORE_CMFUNC_H
+
+
+/* ###########################  Core Function Access  ########################### */
+/** \ingroup  CMSIS_Core_FunctionInterface
+    \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
+  @{
+ */
+
+#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+#if (__ARMCC_VERSION < 400677)
+  #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
+#endif
+
+/* intrinsic void __enable_irq();     */
+/* intrinsic void __disable_irq();    */
+
+/** \brief  Get Control Register
+
+    This function returns the content of the Control Register.
+
+    \return               Control Register value
+ */
+__STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+  register uint32_t __regControl         __ASM("control");
+  return(__regControl);
+}
+
+
+/** \brief  Set Control Register
+
+    This function writes the given value to the Control Register.
+
+    \param [in]    control  Control Register value to set
+ */
+__STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+  register uint32_t __regControl         __ASM("control");
+  __regControl = control;
+}
+
+
+/** \brief  Get IPSR Register
+
+    This function returns the content of the IPSR Register.
+
+    \return               IPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_IPSR(void)
+{
+  register uint32_t __regIPSR          __ASM("ipsr");
+  return(__regIPSR);
+}
+
+
+/** \brief  Get APSR Register
+
+    This function returns the content of the APSR Register.
+
+    \return               APSR Register value
+ */
+__STATIC_INLINE uint32_t __get_APSR(void)
+{
+  register uint32_t __regAPSR          __ASM("apsr");
+  return(__regAPSR);
+}
+
+
+/** \brief  Get xPSR Register
+
+    This function returns the content of the xPSR Register.
+
+    \return               xPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_xPSR(void)
+{
+  register uint32_t __regXPSR          __ASM("xpsr");
+  return(__regXPSR);
+}
+
+
+/** \brief  Get Process Stack Pointer
+
+    This function returns the current value of the Process Stack Pointer (PSP).
+
+    \return               PSP Register value
+ */
+__STATIC_INLINE uint32_t __get_PSP(void)
+{
+  register uint32_t __regProcessStackPointer  __ASM("psp");
+  return(__regProcessStackPointer);
+}
+
+
+/** \brief  Set Process Stack Pointer
+
+    This function assigns the given value to the Process Stack Pointer (PSP).
+
+    \param [in]    topOfProcStack  Process Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+  register uint32_t __regProcessStackPointer  __ASM("psp");
+  __regProcessStackPointer = topOfProcStack;
+}
+
+
+/** \brief  Get Main Stack Pointer
+
+    This function returns the current value of the Main Stack Pointer (MSP).
+
+    \return               MSP Register value
+ */
+__STATIC_INLINE uint32_t __get_MSP(void)
+{
+  register uint32_t __regMainStackPointer     __ASM("msp");
+  return(__regMainStackPointer);
+}
+
+
+/** \brief  Set Main Stack Pointer
+
+    This function assigns the given value to the Main Stack Pointer (MSP).
+
+    \param [in]    topOfMainStack  Main Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+  register uint32_t __regMainStackPointer     __ASM("msp");
+  __regMainStackPointer = topOfMainStack;
+}
+
+
+/** \brief  Get Priority Mask
+
+    This function returns the current state of the priority mask bit from the Priority Mask Register.
+
+    \return               Priority Mask value
+ */
+__STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+  register uint32_t __regPriMask         __ASM("primask");
+  return(__regPriMask);
+}
+
+
+/** \brief  Set Priority Mask
+
+    This function assigns the given value to the Priority Mask Register.
+
+    \param [in]    priMask  Priority Mask
+ */
+__STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+  register uint32_t __regPriMask         __ASM("primask");
+  __regPriMask = (priMask);
+}
+
+
+#if       (__CORTEX_M >= 0x03)
+
+/** \brief  Enable FIQ
+
+    This function enables FIQ interrupts by clearing the F-bit in the CPSR.
+    Can only be executed in Privileged modes.
+ */
+#define __enable_fault_irq                __enable_fiq
+
+
+/** \brief  Disable FIQ
+
+    This function disables FIQ interrupts by setting the F-bit in the CPSR.
+    Can only be executed in Privileged modes.
+ */
+#define __disable_fault_irq               __disable_fiq
+
+
+/** \brief  Get Base Priority
+
+    This function returns the current value of the Base Priority register.
+
+    \return               Base Priority register value
+ */
+__STATIC_INLINE uint32_t  __get_BASEPRI(void)
+{
+  register uint32_t __regBasePri         __ASM("basepri");
+  return(__regBasePri);
+}
+
+
+/** \brief  Set Base Priority
+
+    This function assigns the given value to the Base Priority register.
+
+    \param [in]    basePri  Base Priority value to set
+ */
+__STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
+{
+  register uint32_t __regBasePri         __ASM("basepri");
+  __regBasePri = (basePri & 0xff);
+}
+
+
+/** \brief  Get Fault Mask
+
+    This function returns the current value of the Fault Mask register.
+
+    \return               Fault Mask register value
+ */
+__STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+  register uint32_t __regFaultMask       __ASM("faultmask");
+  return(__regFaultMask);
+}
+
+
+/** \brief  Set Fault Mask
+
+    This function assigns the given value to the Fault Mask register.
+
+    \param [in]    faultMask  Fault Mask value to set
+ */
+__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+  register uint32_t __regFaultMask       __ASM("faultmask");
+  __regFaultMask = (faultMask & (uint32_t)1);
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+#if       (__CORTEX_M == 0x04)
+
+/** \brief  Get FPSCR
+
+    This function returns the current value of the Floating Point Status/Control register.
+
+    \return               Floating Point Status/Control register value
+ */
+__STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+  register uint32_t __regfpscr         __ASM("fpscr");
+  return(__regfpscr);
+#else
+   return(0);
+#endif
+}
+
+
+/** \brief  Set FPSCR
+
+    This function assigns the given value to the Floating Point Status/Control register.
+
+    \param [in]    fpscr  Floating Point Status/Control value to set
+ */
+__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+  register uint32_t __regfpscr         __ASM("fpscr");
+  __regfpscr = (fpscr);
+#endif
+}
+
+#endif /* (__CORTEX_M == 0x04) */
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+#include <cmsis_iar.h>
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+#include <cmsis_ccs.h>
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/** \brief  Enable IRQ Interrupts
+
+  This function enables IRQ interrupts by clearing the I-bit in the CPSR.
+  Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void)
+{
+  __ASM volatile ("cpsie i" : : : "memory");
+}
+
+
+/** \brief  Disable IRQ Interrupts
+
+  This function disables IRQ interrupts by setting the I-bit in the CPSR.
+  Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void)
+{
+  __ASM volatile ("cpsid i" : : : "memory");
+}
+
+
+/** \brief  Get Control Register
+
+    This function returns the content of the Control Register.
+
+    \return               Control Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, control" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Control Register
+
+    This function writes the given value to the Control Register.
+
+    \param [in]    control  Control Register value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+  __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");
+}
+
+
+/** \brief  Get IPSR Register
+
+    This function returns the content of the IPSR Register.
+
+    \return               IPSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Get APSR Register
+
+    This function returns the content of the APSR Register.
+
+    \return               APSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, apsr" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Get xPSR Register
+
+    This function returns the content of the xPSR Register.
+
+    \return               xPSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Get Process Stack Pointer
+
+    This function returns the current value of the Process Stack Pointer (PSP).
+
+    \return               PSP Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)
+{
+  register uint32_t result;
+
+  __ASM volatile ("MRS %0, psp\n"  : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Process Stack Pointer
+
+    This function assigns the given value to the Process Stack Pointer (PSP).
+
+    \param [in]    topOfProcStack  Process Stack Pointer value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+  __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp");
+}
+
+
+/** \brief  Get Main Stack Pointer
+
+    This function returns the current value of the Main Stack Pointer (MSP).
+
+    \return               MSP Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)
+{
+  register uint32_t result;
+
+  __ASM volatile ("MRS %0, msp\n" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Main Stack Pointer
+
+    This function assigns the given value to the Main Stack Pointer (MSP).
+
+    \param [in]    topOfMainStack  Main Stack Pointer value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+  __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp");
+}
+
+
+/** \brief  Get Priority Mask
+
+    This function returns the current state of the priority mask bit from the Priority Mask Register.
+
+    \return               Priority Mask value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, primask" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Priority Mask
+
+    This function assigns the given value to the Priority Mask Register.
+
+    \param [in]    priMask  Priority Mask
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+  __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");
+}
+
+
+#if       (__CORTEX_M >= 0x03)
+
+/** \brief  Enable FIQ
+
+    This function enables FIQ interrupts by clearing the F-bit in the CPSR.
+    Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void)
+{
+  __ASM volatile ("cpsie f" : : : "memory");
+}
+
+
+/** \brief  Disable FIQ
+
+    This function disables FIQ interrupts by setting the F-bit in the CPSR.
+    Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void)
+{
+  __ASM volatile ("cpsid f" : : : "memory");
+}
+
+
+/** \brief  Get Base Priority
+
+    This function returns the current value of the Base Priority register.
+
+    \return               Base Priority register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, basepri_max" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Base Priority
+
+    This function assigns the given value to the Base Priority register.
+
+    \param [in]    basePri  Base Priority value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value)
+{
+  __ASM volatile ("MSR basepri, %0" : : "r" (value) : "memory");
+}
+
+
+/** \brief  Get Fault Mask
+
+    This function returns the current value of the Fault Mask register.
+
+    \return               Fault Mask register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Fault Mask
+
+    This function assigns the given value to the Fault Mask register.
+
+    \param [in]    faultMask  Fault Mask value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+  __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+#if       (__CORTEX_M == 0x04)
+
+/** \brief  Get FPSCR
+
+    This function returns the current value of the Floating Point Status/Control register.
+
+    \return               Floating Point Status/Control register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+  uint32_t result;
+
+  /* Empty asm statement works as a scheduling barrier */
+  __ASM volatile ("");
+  __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
+  __ASM volatile ("");
+  return(result);
+#else
+   return(0);
+#endif
+}
+
+
+/** \brief  Set FPSCR
+
+    This function assigns the given value to the Floating Point Status/Control register.
+
+    \param [in]    fpscr  Floating Point Status/Control value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+  /* Empty asm statement works as a scheduling barrier */
+  __ASM volatile ("");
+  __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc");
+  __ASM volatile ("");
+#endif
+}
+
+#endif /* (__CORTEX_M == 0x04) */
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+/*
+ * The CMSIS functions have been implemented as intrinsics in the compiler.
+ * Please use "carm -?i" to get an up to date list of all instrinsics,
+ * Including the CMSIS ones.
+ */
+
+#endif
+
+/*@} end of CMSIS_Core_RegAccFunctions */
+
+
+#endif /* __CORE_CMFUNC_H */
diff --git a/src/lib/mathlib/CMSIS/Include/core_cmInstr.h b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h
new file mode 100644
index 000000000..8946c2c49
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h
@@ -0,0 +1,688 @@
+/**************************************************************************//**
+ * @file     core_cmInstr.h
+ * @brief    CMSIS Cortex-M Core Instruction Access Header File
+ * @version  V3.20
+ * @date     05. March 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+   All rights reserved.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+   - Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+   POSSIBILITY OF SUCH DAMAGE.
+   ---------------------------------------------------------------------------*/
+
+
+#ifndef __CORE_CMINSTR_H
+#define __CORE_CMINSTR_H
+
+
+/* ##########################  Core Instruction Access  ######################### */
+/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
+  Access to dedicated instructions
+  @{
+*/
+
+#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+#if (__ARMCC_VERSION < 400677)
+  #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
+#endif
+
+
+/** \brief  No Operation
+
+    No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+#define __NOP                             __nop
+
+
+/** \brief  Wait For Interrupt
+
+    Wait For Interrupt is a hint instruction that suspends execution
+    until one of a number of events occurs.
+ */
+#define __WFI                             __wfi
+
+
+/** \brief  Wait For Event
+
+    Wait For Event is a hint instruction that permits the processor to enter
+    a low-power state until one of a number of events occurs.
+ */
+#define __WFE                             __wfe
+
+
+/** \brief  Send Event
+
+    Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+#define __SEV                             __sev
+
+
+/** \brief  Instruction Synchronization Barrier
+
+    Instruction Synchronization Barrier flushes the pipeline in the processor,
+    so that all instructions following the ISB are fetched from cache or
+    memory, after the instruction has been completed.
+ */
+#define __ISB()                           __isb(0xF)
+
+
+/** \brief  Data Synchronization Barrier
+
+    This function acts as a special kind of Data Memory Barrier.
+    It completes when all explicit memory accesses before this instruction complete.
+ */
+#define __DSB()                           __dsb(0xF)
+
+
+/** \brief  Data Memory Barrier
+
+    This function ensures the apparent order of the explicit memory operations before
+    and after the instruction, without ensuring their completion.
+ */
+#define __DMB()                           __dmb(0xF)
+
+
+/** \brief  Reverse byte order (32 bit)
+
+    This function reverses the byte order in integer value.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+#define __REV                             __rev
+
+
+/** \brief  Reverse byte order (16 bit)
+
+    This function reverses the byte order in two unsigned short values.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)
+{
+  rev16 r0, r0
+  bx lr
+}
+#endif
+
+/** \brief  Reverse byte order in signed short value
+
+    This function reverses the byte order in a signed short value with sign extension to integer.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int32_t __REVSH(int32_t value)
+{
+  revsh r0, r0
+  bx lr
+}
+#endif
+
+
+/** \brief  Rotate Right in unsigned value (32 bit)
+
+    This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+
+    \param [in]    value  Value to rotate
+    \param [in]    value  Number of Bits to rotate
+    \return               Rotated value
+ */
+#define __ROR                             __ror
+
+
+/** \brief  Breakpoint
+
+    This function causes the processor to enter Debug state.
+    Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+
+    \param [in]    value  is ignored by the processor.
+                   If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value)                       __breakpoint(value)
+
+
+#if       (__CORTEX_M >= 0x03)
+
+/** \brief  Reverse bit order of value
+
+    This function reverses the bit order of the given value.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+#define __RBIT                            __rbit
+
+
+/** \brief  LDR Exclusive (8 bit)
+
+    This function performs a exclusive LDR command for 8 bit value.
+
+    \param [in]    ptr  Pointer to data
+    \return             value of type uint8_t at (*ptr)
+ */
+#define __LDREXB(ptr)                     ((uint8_t ) __ldrex(ptr))
+
+
+/** \brief  LDR Exclusive (16 bit)
+
+    This function performs a exclusive LDR command for 16 bit values.
+
+    \param [in]    ptr  Pointer to data
+    \return        value of type uint16_t at (*ptr)
+ */
+#define __LDREXH(ptr)                     ((uint16_t) __ldrex(ptr))
+
+
+/** \brief  LDR Exclusive (32 bit)
+
+    This function performs a exclusive LDR command for 32 bit values.
+
+    \param [in]    ptr  Pointer to data
+    \return        value of type uint32_t at (*ptr)
+ */
+#define __LDREXW(ptr)                     ((uint32_t ) __ldrex(ptr))
+
+
+/** \brief  STR Exclusive (8 bit)
+
+    This function performs a exclusive STR command for 8 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+#define __STREXB(value, ptr)              __strex(value, ptr)
+
+
+/** \brief  STR Exclusive (16 bit)
+
+    This function performs a exclusive STR command for 16 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+#define __STREXH(value, ptr)              __strex(value, ptr)
+
+
+/** \brief  STR Exclusive (32 bit)
+
+    This function performs a exclusive STR command for 32 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+#define __STREXW(value, ptr)              __strex(value, ptr)
+
+
+/** \brief  Remove the exclusive lock
+
+    This function removes the exclusive lock which is created by LDREX.
+
+ */
+#define __CLREX                           __clrex
+
+
+/** \brief  Signed Saturate
+
+    This function saturates a signed value.
+
+    \param [in]  value  Value to be saturated
+    \param [in]    sat  Bit position to saturate to (1..32)
+    \return             Saturated value
+ */
+#define __SSAT                            __ssat
+
+
+/** \brief  Unsigned Saturate
+
+    This function saturates an unsigned value.
+
+    \param [in]  value  Value to be saturated
+    \param [in]    sat  Bit position to saturate to (0..31)
+    \return             Saturated value
+ */
+#define __USAT                            __usat
+
+
+/** \brief  Count leading zeros
+
+    This function counts the number of leading zeros of a data value.
+
+    \param [in]  value  Value to count the leading zeros
+    \return             number of leading zeros in value
+ */
+#define __CLZ                             __clz
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+#include <cmsis_iar.h>
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+#include <cmsis_ccs.h>
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/* Define macros for porting to both thumb1 and thumb2.
+ * For thumb1, use low register (r0-r7), specified by constrant "l"
+ * Otherwise, use general registers, specified by constrant "r" */
+#if defined (__thumb__) && !defined (__thumb2__)
+#define __CMSIS_GCC_OUT_REG(r) "=l" (r)
+#define __CMSIS_GCC_USE_REG(r) "l" (r)
+#else
+#define __CMSIS_GCC_OUT_REG(r) "=r" (r)
+#define __CMSIS_GCC_USE_REG(r) "r" (r)
+#endif
+
+/** \brief  No Operation
+
+    No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void)
+{
+  __ASM volatile ("nop");
+}
+
+
+/** \brief  Wait For Interrupt
+
+    Wait For Interrupt is a hint instruction that suspends execution
+    until one of a number of events occurs.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void)
+{
+  __ASM volatile ("wfi");
+}
+
+
+/** \brief  Wait For Event
+
+    Wait For Event is a hint instruction that permits the processor to enter
+    a low-power state until one of a number of events occurs.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void)
+{
+  __ASM volatile ("wfe");
+}
+
+
+/** \brief  Send Event
+
+    Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void)
+{
+  __ASM volatile ("sev");
+}
+
+
+/** \brief  Instruction Synchronization Barrier
+
+    Instruction Synchronization Barrier flushes the pipeline in the processor,
+    so that all instructions following the ISB are fetched from cache or
+    memory, after the instruction has been completed.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void)
+{
+  __ASM volatile ("isb");
+}
+
+
+/** \brief  Data Synchronization Barrier
+
+    This function acts as a special kind of Data Memory Barrier.
+    It completes when all explicit memory accesses before this instruction complete.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void)
+{
+  __ASM volatile ("dsb");
+}
+
+
+/** \brief  Data Memory Barrier
+
+    This function ensures the apparent order of the explicit memory operations before
+    and after the instruction, without ensuring their completion.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void)
+{
+  __ASM volatile ("dmb");
+}
+
+
+/** \brief  Reverse byte order (32 bit)
+
+    This function reverses the byte order in integer value.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)
+  return __builtin_bswap32(value);
+#else
+  uint32_t result;
+
+  __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+  return(result);
+#endif
+}
+
+
+/** \brief  Reverse byte order (16 bit)
+
+    This function reverses the byte order in two unsigned short values.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t value)
+{
+  uint32_t result;
+
+  __ASM volatile ("rev16 %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+  return(result);
+}
+
+
+/** \brief  Reverse byte order in signed short value
+
+    This function reverses the byte order in a signed short value with sign extension to integer.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+  return (short)__builtin_bswap16(value);
+#else
+  uint32_t result;
+
+  __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+  return(result);
+#endif
+}
+
+
+/** \brief  Rotate Right in unsigned value (32 bit)
+
+    This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+
+    \param [in]    value  Value to rotate
+    \param [in]    value  Number of Bits to rotate
+    \return               Rotated value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+  return (op1 >> op2) | (op1 << (32 - op2)); 
+}
+
+
+/** \brief  Breakpoint
+
+    This function causes the processor to enter Debug state.
+    Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+
+    \param [in]    value  is ignored by the processor.
+                   If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value)                       __ASM volatile ("bkpt "#value)
+
+
+#if       (__CORTEX_M >= 0x03)
+
+/** \brief  Reverse bit order of value
+
+    This function reverses the bit order of the given value.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint32_t value)
+{
+  uint32_t result;
+
+   __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) );
+   return(result);
+}
+
+
+/** \brief  LDR Exclusive (8 bit)
+
+    This function performs a exclusive LDR command for 8 bit value.
+
+    \param [in]    ptr  Pointer to data
+    \return             value of type uint8_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uint8_t *addr)
+{
+    uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+   __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+       accepted by assembler. So has to use following less efficient pattern.
+    */
+   __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+   return(result);
+}
+
+
+/** \brief  LDR Exclusive (16 bit)
+
+    This function performs a exclusive LDR command for 16 bit values.
+
+    \param [in]    ptr  Pointer to data
+    \return        value of type uint16_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile uint16_t *addr)
+{
+    uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+   __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+       accepted by assembler. So has to use following less efficient pattern.
+    */
+   __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+   return(result);
+}
+
+
+/** \brief  LDR Exclusive (32 bit)
+
+    This function performs a exclusive LDR command for 32 bit values.
+
+    \param [in]    ptr  Pointer to data
+    \return        value of type uint32_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(volatile uint32_t *addr)
+{
+    uint32_t result;
+
+   __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) );
+   return(result);
+}
+
+
+/** \brief  STR Exclusive (8 bit)
+
+    This function performs a exclusive STR command for 8 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)
+{
+   uint32_t result;
+
+   __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+   return(result);
+}
+
+
+/** \brief  STR Exclusive (16 bit)
+
+    This function performs a exclusive STR command for 16 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)
+{
+   uint32_t result;
+
+   __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+   return(result);
+}
+
+
+/** \brief  STR Exclusive (32 bit)
+
+    This function performs a exclusive STR command for 32 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)
+{
+   uint32_t result;
+
+   __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+   return(result);
+}
+
+
+/** \brief  Remove the exclusive lock
+
+    This function removes the exclusive lock which is created by LDREX.
+
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void)
+{
+  __ASM volatile ("clrex" ::: "memory");
+}
+
+
+/** \brief  Signed Saturate
+
+    This function saturates a signed value.
+
+    \param [in]  value  Value to be saturated
+    \param [in]    sat  Bit position to saturate to (1..32)
+    \return             Saturated value
+ */
+#define __SSAT(ARG1,ARG2) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("ssat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+
+/** \brief  Unsigned Saturate
+
+    This function saturates an unsigned value.
+
+    \param [in]  value  Value to be saturated
+    \param [in]    sat  Bit position to saturate to (0..31)
+    \return             Saturated value
+ */
+#define __USAT(ARG1,ARG2) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("usat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+
+/** \brief  Count leading zeros
+
+    This function counts the number of leading zeros of a data value.
+
+    \param [in]  value  Value to count the leading zeros
+    \return             number of leading zeros in value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value)
+{
+   uint32_t result;
+
+  __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
+  return(result);
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+/*
+ * The CMSIS functions have been implemented as intrinsics in the compiler.
+ * Please use "carm -?i" to get an up to date list of all intrinsics,
+ * Including the CMSIS ones.
+ */
+
+#endif
+
+/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
+
+#endif /* __CORE_CMINSTR_H */
diff --git a/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
new file mode 100644
index 000000000..6898bc27d
Binary files /dev/null and b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a differ
diff --git a/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
new file mode 100755
index 000000000..a0185eaa9
Binary files /dev/null and b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a differ
diff --git a/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
new file mode 100755
index 000000000..94525528e
Binary files /dev/null and b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a differ
diff --git a/src/lib/mathlib/CMSIS/library.mk b/src/lib/mathlib/CMSIS/library.mk
new file mode 100644
index 000000000..0cc1b559d
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/library.mk
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# ARM CMSIS DSP library
+#
+
+ifeq ($(CONFIG_ARCH),CORTEXM4F)
+PREBUILT_LIB		:= libarm_cortexM4lf_math.a
+else ifeq ($(CONFIG_ARCH),CORTEXM4)
+PREBUILT_LIB		:= libarm_cortexM4l_math.a
+else ifeq ($(CONFIG_ARCH),CORTEXM3)
+PREBUILT_LIB		:= libarm_cortexM3l_math.a
+else
+$(error CONFIG_ARCH value '$(CONFIG_ARCH)' not supported by the DSP library)
+endif
diff --git a/src/lib/mathlib/CMSIS/license.txt b/src/lib/mathlib/CMSIS/license.txt
new file mode 100644
index 000000000..31afac1ec
--- /dev/null
+++ b/src/lib/mathlib/CMSIS/license.txt
@@ -0,0 +1,27 @@
+All pre-built libraries are guided by the following license:
+
+Copyright (C) 2009-2012 ARM Limited. 
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+ - 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.
+ - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp
new file mode 100644
index 000000000..f509f7081
--- /dev/null
+++ b/src/lib/mathlib/math/Dcm.cpp
@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Dcm.cpp
+ *
+ * math direction cosine matrix
+ */
+
+#include <mathlib/math/test/test.hpp>
+
+#include "Dcm.hpp"
+#include "Quaternion.hpp"
+#include "EulerAngles.hpp"
+#include "Vector3.hpp"
+
+namespace math
+{
+
+Dcm::Dcm() :
+	Matrix(Matrix::identity(3))
+{
+}
+
+Dcm::Dcm(float c00, float c01, float c02,
+	 float c10, float c11, float c12,
+	 float c20, float c21, float c22) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	dcm(0, 0) = c00;
+	dcm(0, 1) = c01;
+	dcm(0, 2) = c02;
+	dcm(1, 0) = c10;
+	dcm(1, 1) = c11;
+	dcm(1, 2) = c12;
+	dcm(2, 0) = c20;
+	dcm(2, 1) = c21;
+	dcm(2, 2) = c22;
+}
+
+Dcm::Dcm(const float data[3][3]) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	/* set rotation matrix */
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		dcm(i, j) = data[i][j];
+}
+
+Dcm::Dcm(const float *data) :
+	Matrix(3, 3, data)
+{
+}
+
+Dcm::Dcm(const Quaternion &q) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	double a = q.getA();
+	double b = q.getB();
+	double c = q.getC();
+	double d = q.getD();
+	double aSq = a * a;
+	double bSq = b * b;
+	double cSq = c * c;
+	double dSq = d * d;
+	dcm(0, 0) = aSq + bSq - cSq - dSq;
+	dcm(0, 1) = 2.0 * (b * c - a * d);
+	dcm(0, 2) = 2.0 * (a * c + b * d);
+	dcm(1, 0) = 2.0 * (b * c + a * d);
+	dcm(1, 1) = aSq - bSq + cSq - dSq;
+	dcm(1, 2) = 2.0 * (c * d - a * b);
+	dcm(2, 0) = 2.0 * (b * d - a * c);
+	dcm(2, 1) = 2.0 * (a * b + c * d);
+	dcm(2, 2) = aSq - bSq - cSq + dSq;
+}
+
+Dcm::Dcm(const EulerAngles &euler) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	double cosPhi = cos(euler.getPhi());
+	double sinPhi = sin(euler.getPhi());
+	double cosThe = cos(euler.getTheta());
+	double sinThe = sin(euler.getTheta());
+	double cosPsi = cos(euler.getPsi());
+	double sinPsi = sin(euler.getPsi());
+
+	dcm(0, 0) = cosThe * cosPsi;
+	dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
+	dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
+
+	dcm(1, 0) = cosThe * sinPsi;
+	dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
+	dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
+
+	dcm(2, 0) = -sinThe;
+	dcm(2, 1) = sinPhi * cosThe;
+	dcm(2, 2) = cosPhi * cosThe;
+}
+
+Dcm::Dcm(const Dcm &right) :
+	Matrix(right)
+{
+}
+
+Dcm::~Dcm()
+{
+}
+
+int __EXPORT dcmTest()
+{
+	printf("Test DCM\t\t: ");
+	// default ctor
+	ASSERT(matrixEqual(Dcm(),
+			   Matrix::identity(3)));
+	// quaternion ctor
+	ASSERT(matrixEqual(
+		       Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
+		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
+			   0.2896295f,  0.9564251f, -0.0369570f,
+			   -0.1986693f,  0.0978434f,  0.9751703f)));
+	// euler angle ctor
+	ASSERT(matrixEqual(
+		       Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
+		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
+			   0.2896295f,  0.9564251f, -0.0369570f,
+			   -0.1986693f,  0.0978434f,  0.9751703f)));
+	// rotations
+	Vector3 vB(1, 2, 3);
+	ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
+			   Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
+	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
+			   Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
+	ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
+			   Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
+	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
+			   Dcm(EulerAngles(
+				       M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
+	printf("PASS\n");
+	return 0;
+}
+} // namespace math
diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp
new file mode 100644
index 000000000..df8970d3a
--- /dev/null
+++ b/src/lib/mathlib/math/Dcm.hpp
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   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 Dcm.hpp
+ *
+ * math direction cosine matrix
+ */
+
+//#pragma once
+
+#include "Vector.hpp"
+#include "Matrix.hpp"
+
+namespace math
+{
+
+class Quaternion;
+class EulerAngles;
+
+/**
+ * This is a Tait Bryan, Body 3-2-1 sequence.
+ * (yaw)-(pitch)-(roll)
+ * The Dcm transforms a vector in the body frame
+ * to the navigation frame, typically represented
+ * as C_nb. C_bn can be obtained through use
+ * of the transpose() method.
+ */
+class __EXPORT Dcm : public Matrix
+{
+public:
+	/**
+	 * default ctor
+	 */
+	Dcm();
+
+	/**
+	 * scalar ctor
+	 */
+	Dcm(float c00, float c01, float c02,
+	    float c10, float c11, float c12,
+	    float c20, float c21, float c22);
+
+	/**
+	 * data ctor
+	 */
+	Dcm(const float *data);
+
+	/**
+	 * array ctor
+	 */
+	Dcm(const float data[3][3]);
+
+	/**
+	 * quaternion ctor
+	 */
+	Dcm(const Quaternion &q);
+
+	/**
+	 * euler angles ctor
+	 */
+	Dcm(const EulerAngles &euler);
+
+	/**
+	 * copy ctor (deep)
+	 */
+	Dcm(const Dcm &right);
+
+	/**
+	 * dtor
+	 */
+	virtual ~Dcm();
+};
+
+int __EXPORT dcmTest();
+
+} // math
+
diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp
new file mode 100644
index 000000000..e733d23bb
--- /dev/null
+++ b/src/lib/mathlib/math/EulerAngles.cpp
@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   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 Vector.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "EulerAngles.hpp"
+#include "Quaternion.hpp"
+#include "Dcm.hpp"
+#include "Vector3.hpp"
+
+namespace math
+{
+
+EulerAngles::EulerAngles() :
+	Vector(3)
+{
+	setPhi(0.0f);
+	setTheta(0.0f);
+	setPsi(0.0f);
+}
+
+EulerAngles::EulerAngles(float phi, float theta, float psi) :
+	Vector(3)
+{
+	setPhi(phi);
+	setTheta(theta);
+	setPsi(psi);
+}
+
+EulerAngles::EulerAngles(const Quaternion &q) :
+	Vector(3)
+{
+	(*this) = EulerAngles(Dcm(q));
+}
+
+EulerAngles::EulerAngles(const Dcm &dcm) :
+	Vector(3)
+{
+	setTheta(asinf(-dcm(2, 0)));
+
+	if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
+		setPhi(0.0f);
+		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
+			      dcm(0, 2) + dcm(1, 1)) + getPhi());
+
+	} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
+		setPhi(0.0f);
+		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
+			      dcm(0, 2) + dcm(1, 1)) - getPhi());
+
+	} else {
+		setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
+		setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
+	}
+}
+
+EulerAngles::~EulerAngles()
+{
+}
+
+int __EXPORT eulerAnglesTest()
+{
+	printf("Test EulerAngles\t: ");
+	EulerAngles euler(0.1f, 0.2f, 0.3f);
+
+	// test ctor
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+	ASSERT(equal(euler.getPhi(), 0.1f));
+	ASSERT(equal(euler.getTheta(), 0.2f));
+	ASSERT(equal(euler.getPsi(), 0.3f));
+
+	// test dcm ctor
+	euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+
+	// test quat ctor
+	euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+
+	// test assignment
+	euler.setPhi(0.4f);
+	euler.setTheta(0.5f);
+	euler.setPsi(0.6f);
+	ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
+
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math
diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp
new file mode 100644
index 000000000..399eecfa7
--- /dev/null
+++ b/src/lib/mathlib/math/EulerAngles.hpp
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+class Quaternion;
+class Dcm;
+
+class __EXPORT EulerAngles : public Vector
+{
+public:
+	EulerAngles();
+	EulerAngles(float phi, float theta, float psi);
+	EulerAngles(const Quaternion &q);
+	EulerAngles(const Dcm &dcm);
+	virtual ~EulerAngles();
+
+	// alias
+	void setPhi(float phi) { (*this)(0) = phi; }
+	void setTheta(float theta) { (*this)(1) = theta; }
+	void setPsi(float psi) { (*this)(2) = psi; }
+
+	// const accessors
+	const float &getPhi() const { return (*this)(0); }
+	const float &getTheta() const { return (*this)(1); }
+	const float &getPsi() const { return (*this)(2); }
+
+};
+
+int __EXPORT eulerAnglesTest();
+
+} // math
+
diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
new file mode 100644
index 000000000..d4c892d8a
--- /dev/null
+++ b/src/lib/mathlib/math/Limits.cpp
@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Limits.cpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+
+#include <math.h>
+#include <stdint.h>
+
+#include "Limits.hpp"
+
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+int __EXPORT min(int val1, int val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+unsigned __EXPORT min(unsigned val1, unsigned val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+double __EXPORT min(double val1, double val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+float __EXPORT max(float val1, float val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+int __EXPORT max(int val1, int val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+unsigned __EXPORT max(unsigned val1, unsigned val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+double __EXPORT max(double val1, double val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+
+float __EXPORT constrain(float val, float min, float max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+int __EXPORT constrain(int val, int min, int max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+double __EXPORT constrain(double val, double min, double max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+float __EXPORT radians(float degrees)
+{
+	return (degrees / 180.0f) * M_PI_F;
+}
+
+double __EXPORT radians(double degrees)
+{
+	return (degrees / 180.0) * M_PI;
+}
+
+float __EXPORT degrees(float radians)
+{
+	return (radians / M_PI_F) * 180.0f;
+}
+
+double __EXPORT degrees(double radians)
+{
+	return (radians / M_PI) * 180.0;
+}
+
+}
\ No newline at end of file
diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
new file mode 100644
index 000000000..fb778dd66
--- /dev/null
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Limits.hpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+#pragma once
+
+#include <nuttx/config.h>
+#include <stdint.h>
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2);
+
+int __EXPORT min(int val1, int val2);
+
+unsigned __EXPORT min(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2);
+
+double __EXPORT min(double val1, double val2);
+
+float __EXPORT max(float val1, float val2);
+
+int __EXPORT max(int val1, int val2);
+
+unsigned __EXPORT max(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2);
+
+double __EXPORT max(double val1, double val2);
+
+
+float __EXPORT constrain(float val, float min, float max);
+
+int __EXPORT constrain(int val, int min, int max);
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max);
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max);
+
+double __EXPORT constrain(double val, double min, double max);
+
+float __EXPORT radians(float degrees);
+
+double __EXPORT radians(double degrees);
+
+float __EXPORT degrees(float radians);
+
+double __EXPORT degrees(double radians);
+
+}
\ No newline at end of file
diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp
new file mode 100644
index 000000000..ebd1aeda3
--- /dev/null
+++ b/src/lib/mathlib/math/Matrix.cpp
@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   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 Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include "test/test.hpp"
+#include <math.h>
+
+#include "Matrix.hpp"
+
+namespace math
+{
+
+static const float data_testA[] = {
+	1, 2, 3,
+	4, 5, 6
+};
+static Matrix testA(2, 3, data_testA);
+
+static const float data_testB[] = {
+	0, 1, 3,
+	7, -1, 2
+};
+static Matrix testB(2, 3, data_testB);
+
+static const float data_testC[] = {
+	0, 1,
+	2, 1,
+	3, 2
+};
+static Matrix testC(3, 2, data_testC);
+
+static const float data_testD[] = {
+	0, 1, 2,
+	2, 1, 4,
+	5, 2, 0
+};
+static Matrix testD(3, 3, data_testD);
+
+static const float data_testE[] = {
+	1, -1, 2,
+	0, 2, 3,
+	2, -1, 1
+};
+static Matrix testE(3, 3, data_testE);
+
+static const float data_testF[] = {
+	3.777e006f, 2.915e007f, 0.000e000f,
+	2.938e007f, 2.267e008f, 0.000e000f,
+	0.000e000f, 0.000e000f, 6.033e008f
+};
+static Matrix testF(3, 3, data_testF);
+
+int __EXPORT matrixTest()
+{
+	matrixAddTest();
+	matrixSubTest();
+	matrixMultTest();
+	matrixInvTest();
+	matrixDivTest();
+	return 0;
+}
+
+int matrixAddTest()
+{
+	printf("Test Matrix Add\t\t: ");
+	Matrix r = testA + testB;
+	float data_test[] = {
+		1.0f, 3.0f, 6.0f,
+		11.0f, 4.0f, 8.0f
+	};
+	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+int matrixSubTest()
+{
+	printf("Test Matrix Sub\t\t: ");
+	Matrix r = testA - testB;
+	float data_test[] = {
+		1.0f, 1.0f, 0.0f,
+		-3.0f, 6.0f, 4.0f
+	};
+	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+int matrixMultTest()
+{
+	printf("Test Matrix Mult\t: ");
+	Matrix r = testC * testB;
+	float data_test[] = {
+		7.0f, -1.0f,  2.0f,
+		7.0f,  1.0f,  8.0f,
+		14.0f,  1.0f, 13.0f
+	};
+	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+int matrixInvTest()
+{
+	printf("Test Matrix Inv\t\t: ");
+	Matrix origF = testF;
+	Matrix r = testF.inverse();
+	float data_test[] = {
+		-0.0012518f,  0.0001610f, 0.0000000f,
+		0.0001622f, -0.0000209f, 0.0000000f,
+		0.0000000f,  0.0000000f, 1.6580e-9f
+	};
+	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
+	// make sure F in unchanged
+	ASSERT(matrixEqual(origF, testF));
+	printf("PASS\n");
+	return 0;
+}
+
+int matrixDivTest()
+{
+	printf("Test Matrix Div\t\t: ");
+	Matrix r = testD / testE;
+	float data_test[] = {
+		0.2222222f, 0.5555556f, -0.1111111f,
+		0.0f,       1.0f,         1.0,
+		-4.1111111f, 1.2222222f,  4.5555556f
+	};
+	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+bool matrixEqual(const Matrix &a, const Matrix &b, float eps)
+{
+	if (a.getRows() != b.getRows()) {
+		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
+		return false;
+
+	} else if (a.getCols() != b.getCols()) {
+		printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
+		return false;
+	}
+
+	bool ret = true;
+
+	for (size_t i = 0; i < a.getRows(); i++)
+		for (size_t j = 0; j < a.getCols(); j++) {
+			if (!equal(a(i, j), b(i, j), eps)) {
+				printf("element mismatch (%d, %d)\n", i, j);
+				ret = false;
+			}
+		}
+
+	return ret;
+}
+
+} // namespace math
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
new file mode 100644
index 000000000..f19db15ec
--- /dev/null
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   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 Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+#include <nuttx/config.h>
+
+#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
+#include "arm/Matrix.hpp"
+#else
+#include "generic/Matrix.hpp"
+#endif
+
+namespace math
+{
+class Matrix;
+int matrixTest();
+int matrixAddTest();
+int matrixSubTest();
+int matrixMultTest();
+int matrixInvTest();
+int matrixDivTest();
+int matrixArmTest();
+bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
+} // namespace math
diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp
new file mode 100644
index 000000000..02fec4ca6
--- /dev/null
+++ b/src/lib/mathlib/math/Quaternion.cpp
@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Quaternion.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+
+#include "Quaternion.hpp"
+#include "Dcm.hpp"
+#include "EulerAngles.hpp"
+
+namespace math
+{
+
+Quaternion::Quaternion() :
+	Vector(4)
+{
+	setA(1.0f);
+	setB(0.0f);
+	setC(0.0f);
+	setD(0.0f);
+}
+
+Quaternion::Quaternion(float a, float b,
+		       float c, float d) :
+	Vector(4)
+{
+	setA(a);
+	setB(b);
+	setC(c);
+	setD(d);
+}
+
+Quaternion::Quaternion(const float *data) :
+	Vector(4, data)
+{
+}
+
+Quaternion::Quaternion(const Vector &v) :
+	Vector(v)
+{
+}
+
+Quaternion::Quaternion(const Dcm &dcm) :
+	Vector(4)
+{
+	// avoiding singularities by not using
+	// division equations
+	setA(0.5 * sqrt(1.0 +
+			double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
+	setB(0.5 * sqrt(1.0 +
+			double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
+	setC(0.5 * sqrt(1.0 +
+			double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
+	setD(0.5 * sqrt(1.0 +
+			double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
+}
+
+Quaternion::Quaternion(const EulerAngles &euler) :
+	Vector(4)
+{
+	double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
+	double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
+	double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
+	double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
+	double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
+	double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
+	setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
+	     sinPhi_2 * sinTheta_2 * sinPsi_2);
+	setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
+	     cosPhi_2 * sinTheta_2 * sinPsi_2);
+	setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
+	     sinPhi_2 * cosTheta_2 * sinPsi_2);
+	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
+	     sinPhi_2 * sinTheta_2 * cosPsi_2);
+}
+
+Quaternion::Quaternion(const Quaternion &right) :
+	Vector(right)
+{
+}
+
+Quaternion::~Quaternion()
+{
+}
+
+Vector Quaternion::derivative(const Vector &w)
+{
+#ifdef QUATERNION_ASSERT
+	ASSERT(w.getRows() == 3);
+#endif
+	float dataQ[] = {
+		getA(), -getB(), -getC(), -getD(),
+		getB(),  getA(), -getD(),  getC(),
+		getC(),  getD(),  getA(), -getB(),
+		getD(), -getC(),  getB(),  getA()
+	};
+	Vector v(4);
+	v(0) = 0.0f;
+	v(1) = w(0);
+	v(2) = w(1);
+	v(3) = w(2);
+	Matrix Q(4, 4, dataQ);
+	return Q * v * 0.5f;
+}
+
+int __EXPORT quaternionTest()
+{
+	printf("Test Quaternion\t\t: ");
+	// test default ctor
+	Quaternion q;
+	ASSERT(equal(q.getA(), 1.0f));
+	ASSERT(equal(q.getB(), 0.0f));
+	ASSERT(equal(q.getC(), 0.0f));
+	ASSERT(equal(q.getD(), 0.0f));
+	// test float ctor
+	q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
+	ASSERT(equal(q.getA(), 0.1825742f));
+	ASSERT(equal(q.getB(), 0.3651484f));
+	ASSERT(equal(q.getC(), 0.5477226f));
+	ASSERT(equal(q.getD(), 0.7302967f));
+	// test euler ctor
+	q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
+	// test dcm ctor
+	q = Quaternion(Dcm());
+	ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
+	// TODO test derivative
+	// test accessors
+	q.setA(0.1f);
+	q.setB(0.2f);
+	q.setC(0.3f);
+	q.setD(0.4f);
+	ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
new file mode 100644
index 000000000..048a55d33
--- /dev/null
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Quaternion.hpp
+ *
+ * math quaternion lib
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+#include "Matrix.hpp"
+
+namespace math
+{
+
+class Dcm;
+class EulerAngles;
+
+class __EXPORT Quaternion : public Vector
+{
+public:
+
+	/**
+	 * default ctor
+	 */
+	Quaternion();
+
+	/**
+	 * ctor from floats
+	 */
+	Quaternion(float a, float b, float c, float d);
+
+	/**
+	 * ctor from data
+	 */
+	Quaternion(const float *data);
+
+	/**
+	 * ctor from Vector
+	 */
+	Quaternion(const Vector &v);
+
+	/**
+	 * ctor from EulerAngles
+	 */
+	Quaternion(const EulerAngles &euler);
+
+	/**
+	 * ctor from Dcm
+	 */
+	Quaternion(const Dcm &dcm);
+
+	/**
+	 * deep copy ctor
+	 */
+	Quaternion(const Quaternion &right);
+
+	/**
+	 * dtor
+	 */
+	virtual ~Quaternion();
+
+	/**
+	 * derivative
+	 */
+	Vector derivative(const Vector &w);
+
+	/**
+	 * accessors
+	 */
+	void setA(float a) { (*this)(0) = a; }
+	void setB(float b) { (*this)(1) = b; }
+	void setC(float c) { (*this)(2) = c; }
+	void setD(float d) { (*this)(3) = d; }
+	const float &getA() const { return (*this)(0); }
+	const float &getB() const { return (*this)(1); }
+	const float &getC() const { return (*this)(2); }
+	const float &getD() const { return (*this)(3); }
+};
+
+int __EXPORT quaternionTest();
+} // math
+
diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp
new file mode 100644
index 000000000..35158a396
--- /dev/null
+++ b/src/lib/mathlib/math/Vector.cpp
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   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 Vector.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+static const float data_testA[] = {1, 3};
+static const float data_testB[] = {4, 1};
+
+static Vector testA(2, data_testA);
+static Vector testB(2, data_testB);
+
+int __EXPORT vectorTest()
+{
+	vectorAddTest();
+	vectorSubTest();
+	return 0;
+}
+
+int vectorAddTest()
+{
+	printf("Test Vector Add\t\t: ");
+	Vector r = testA + testB;
+	float data_test[] = {5.0f, 4.0f};
+	ASSERT(vectorEqual(Vector(2, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+int vectorSubTest()
+{
+	printf("Test Vector Sub\t\t: ");
+	Vector r(2);
+	r = testA - testB;
+	float data_test[] = { -3.0f, 2.0f};
+	ASSERT(vectorEqual(Vector(2, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+bool vectorEqual(const Vector &a, const Vector &b, float eps)
+{
+	if (a.getRows() != b.getRows()) {
+		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
+		return false;
+	}
+
+	bool ret = true;
+
+	for (size_t i = 0; i < a.getRows(); i++) {
+		if (!equal(a(i), b(i), eps)) {
+			printf("element mismatch (%d)\n", i);
+			ret = false;
+		}
+	}
+
+	return ret;
+}
+
+} // namespace math
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
new file mode 100644
index 000000000..73de793d5
--- /dev/null
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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 Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include <nuttx/config.h>
+
+#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
+#include "arm/Vector.hpp"
+#else
+#include "generic/Vector.hpp"
+#endif
+
+namespace math
+{
+class Vector;
+int __EXPORT vectorTest();
+int __EXPORT vectorAddTest();
+int __EXPORT vectorSubTest();
+bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
+} // math
diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp
new file mode 100644
index 000000000..68e741817
--- /dev/null
+++ b/src/lib/mathlib/math/Vector2f.cpp
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector2f.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "Vector2f.hpp"
+
+namespace math
+{
+
+Vector2f::Vector2f() :
+	Vector(2)
+{
+}
+
+Vector2f::Vector2f(const Vector &right) :
+	Vector(right)
+{
+#ifdef VECTOR_ASSERT
+	ASSERT(right.getRows() == 2);
+#endif
+}
+
+Vector2f::Vector2f(float x, float y) :
+	Vector(2)
+{
+	setX(x);
+	setY(y);
+}
+
+Vector2f::Vector2f(const float *data) :
+	Vector(2, data)
+{
+}
+
+Vector2f::~Vector2f()
+{
+}
+
+float Vector2f::cross(const Vector2f &b) const
+{
+	const Vector2f &a = *this;
+	return a(0)*b(1) - a(1)*b(0);
+}
+
+float Vector2f::operator %(const Vector2f &v) const
+{
+	return cross(v);
+}
+    
+float Vector2f::operator *(const Vector2f &v) const
+{
+    return dot(v);
+}
+
+int __EXPORT vector2fTest()
+{
+	printf("Test Vector2f\t\t: ");
+	// test float ctor
+	Vector2f v(1, 2);
+	ASSERT(equal(v(0), 1));
+	ASSERT(equal(v(1), 2));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math
diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp
new file mode 100644
index 000000000..ecd62e81c
--- /dev/null
+++ b/src/lib/mathlib/math/Vector2f.hpp
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector2f.hpp
+ *
+ * math 3 vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+class __EXPORT Vector2f :
+	public Vector
+{
+public:
+	Vector2f();
+	Vector2f(const Vector &right);
+	Vector2f(float x, float y);
+	Vector2f(const float *data);
+	virtual ~Vector2f();
+	float cross(const Vector2f &b) const;
+	float operator %(const Vector2f &v) const;
+    float operator *(const Vector2f &v) const;
+    inline Vector2f operator*(const float &right) const {
+		return Vector::operator*(right);
+	}
+
+	/**
+	 * accessors
+	 */
+	void setX(float x) { (*this)(0) = x; }
+	void setY(float y) { (*this)(1) = y; }
+	const float &getX() const { return (*this)(0); }
+	const float &getY() const { return (*this)(1); }
+};
+    
+class __EXPORT Vector2 :
+	public Vector2f
+{
+};
+
+int __EXPORT vector2fTest();
+} // math
+
diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp
new file mode 100644
index 000000000..dcb85600e
--- /dev/null
+++ b/src/lib/mathlib/math/Vector3.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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 Vector3.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "Vector3.hpp"
+
+namespace math
+{
+
+Vector3::Vector3() :
+	Vector(3)
+{
+}
+
+Vector3::Vector3(const Vector &right) :
+	Vector(right)
+{
+#ifdef VECTOR_ASSERT
+	ASSERT(right.getRows() == 3);
+#endif
+}
+
+Vector3::Vector3(float x, float y, float z) :
+	Vector(3)
+{
+	setX(x);
+	setY(y);
+	setZ(z);
+}
+
+Vector3::Vector3(const float *data) :
+	Vector(3, data)
+{
+}
+
+Vector3::~Vector3()
+{
+}
+
+Vector3 Vector3::cross(const Vector3 &b) const
+{
+	const Vector3 &a = *this;
+	Vector3 result;
+	result(0) = a(1) * b(2) - a(2) * b(1);
+	result(1) = a(2) * b(0) - a(0) * b(2);
+	result(2) = a(0) * b(1) - a(1) * b(0);
+	return result;
+}
+
+int __EXPORT vector3Test()
+{
+	printf("Test Vector3\t\t: ");
+	// test float ctor
+	Vector3 v(1, 2, 3);
+	ASSERT(equal(v(0), 1));
+	ASSERT(equal(v(1), 2));
+	ASSERT(equal(v(2), 3));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math
diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp
new file mode 100644
index 000000000..568d9669a
--- /dev/null
+++ b/src/lib/mathlib/math/Vector3.hpp
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   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 Vector3.hpp
+ *
+ * math 3 vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+class __EXPORT Vector3 :
+	public Vector
+{
+public:
+	Vector3();
+	Vector3(const Vector &right);
+	Vector3(float x, float y, float z);
+	Vector3(const float *data);
+	virtual ~Vector3();
+	Vector3 cross(const Vector3 &b) const;
+
+	/**
+	 * accessors
+	 */
+	void setX(float x) { (*this)(0) = x; }
+	void setY(float y) { (*this)(1) = y; }
+	void setZ(float z) { (*this)(2) = z; }
+	const float &getX() const { return (*this)(0); }
+	const float &getY() const { return (*this)(1); }
+	const float &getZ() const { return (*this)(2); }
+};
+    
+class __EXPORT Vector3f :
+	public Vector3
+{
+};
+
+int __EXPORT vector3Test();
+} // math
+
diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp
new file mode 100644
index 000000000..21661622a
--- /dev/null
+++ b/src/lib/mathlib/math/arm/Matrix.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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 Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include "Matrix.hpp"
diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
new file mode 100644
index 000000000..715fd3a5e
--- /dev/null
+++ b/src/lib/mathlib/math/arm/Matrix.hpp
@@ -0,0 +1,292 @@
+/****************************************************************************
+ *
+ *   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 Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+#include "../Matrix.hpp"
+
+// arm specific
+#include "../../CMSIS/Include/arm_math.h"
+
+namespace math
+{
+
+class __EXPORT Matrix
+{
+public:
+	// constructor
+	Matrix(size_t rows, size_t cols) :
+		_matrix() {
+		arm_mat_init_f32(&_matrix,
+				 rows, cols,
+				 (float *)calloc(rows * cols, sizeof(float)));
+	}
+	Matrix(size_t rows, size_t cols, const float *data) :
+		_matrix() {
+		arm_mat_init_f32(&_matrix,
+				 rows, cols,
+				 (float *)malloc(rows * cols * sizeof(float)));
+		memcpy(getData(), data, getSize());
+	}
+	// deconstructor
+	virtual ~Matrix() {
+		delete [] _matrix.pData;
+	}
+	// copy constructor (deep)
+	Matrix(const Matrix &right) :
+		_matrix() {
+		arm_mat_init_f32(&_matrix,
+				 right.getRows(), right.getCols(),
+				 (float *)malloc(right.getRows()*
+						 right.getCols()*sizeof(float)));
+		memcpy(getData(), right.getData(),
+		       getSize());
+	}
+	// assignment
+	inline Matrix &operator=(const Matrix &right) {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+
+		if (this != &right) {
+			memcpy(getData(), right.getData(),
+			       right.getSize());
+		}
+
+		return *this;
+	}
+	// element accessors
+	inline float &operator()(size_t i, size_t j) {
+#ifdef MATRIX_ASSERT
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
+#endif
+		return getData()[i * getCols() + j];
+	}
+	inline const float &operator()(size_t i, size_t j) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
+#endif
+		return getData()[i * getCols() + j];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				float sig;
+				int exp;
+				float num = (*this)(i, j);
+				float2SigExp(num, sig, exp);
+				printf("%6.3fe%03.3d,", (double)sig, exp);
+			}
+
+			printf("\n");
+		}
+	}
+	// boolean ops
+	inline bool operator==(const Matrix &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
+					return false;
+			}
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Matrix operator+(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_offset_f32((float *)getData(), right,
+			       (float *)result.getData(), getRows()*getCols());
+		return result;
+	}
+	inline Matrix operator-(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_offset_f32((float *)getData(), -right,
+			       (float *)result.getData(), getRows()*getCols());
+		return result;
+	}
+	inline Matrix operator*(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_mat_scale_f32(&_matrix, right,
+				  &(result._matrix));
+		return result;
+	}
+	inline Matrix operator/(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_mat_scale_f32(&_matrix, 1.0f / right,
+				  &(result._matrix));
+		return result;
+	}
+	// vector ops
+	inline Vector operator*(const Vector &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getCols() == right.getRows());
+#endif
+		Matrix resultMat = (*this) *
+				   Matrix(right.getRows(), 1, right.getData());
+		return Vector(getRows(), resultMat.getData());
+	}
+	// matrix ops
+	inline Matrix operator+(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+		Matrix result(getRows(), getCols());
+		arm_mat_add_f32(&_matrix, &(right._matrix),
+				&(result._matrix));
+		return result;
+	}
+	inline Matrix operator-(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+		Matrix result(getRows(), getCols());
+		arm_mat_sub_f32(&_matrix, &(right._matrix),
+				&(result._matrix));
+		return result;
+	}
+	inline Matrix operator*(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getCols() == right.getRows());
+#endif
+		Matrix result(getRows(), right.getCols());
+		arm_mat_mult_f32(&_matrix, &(right._matrix),
+				 &(result._matrix));
+		return result;
+	}
+	inline Matrix operator/(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(right.getRows() == right.getCols());
+		ASSERT(getCols() == right.getCols());
+#endif
+		return (*this) * right.inverse();
+	}
+	// other functions
+	inline Matrix transpose() const {
+		Matrix result(getCols(), getRows());
+		arm_mat_trans_f32(&_matrix, &(result._matrix));
+		return result;
+	}
+	inline void swapRows(size_t a, size_t b) {
+		if (a == b) return;
+
+		for (size_t j = 0; j < getCols(); j++) {
+			float tmp = (*this)(a, j);
+			(*this)(a, j) = (*this)(b, j);
+			(*this)(b, j) = tmp;
+		}
+	}
+	inline void swapCols(size_t a, size_t b) {
+		if (a == b) return;
+
+		for (size_t i = 0; i < getRows(); i++) {
+			float tmp = (*this)(i, a);
+			(*this)(i, a) = (*this)(i, b);
+			(*this)(i, b) = tmp;
+		}
+	}
+	/**
+	 * inverse based on LU factorization with partial pivotting
+	 */
+	Matrix inverse() const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == getCols());
+#endif
+		Matrix result(getRows(), getCols());
+		Matrix work = (*this);
+		arm_mat_inverse_f32(&(work._matrix),
+				    &(result._matrix));
+		return result;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				(*this)(i, j) = val;
+			}
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _matrix.numRows; }
+	inline size_t getCols() const { return _matrix.numCols; }
+	inline static Matrix identity(size_t size) {
+		Matrix result(size, size);
+
+		for (size_t i = 0; i < size; i++) {
+			result(i, i) = 1.0f;
+		}
+
+		return result;
+	}
+	inline static Matrix zero(size_t size) {
+		Matrix result(size, size);
+		result.setAll(0.0f);
+		return result;
+	}
+	inline static Matrix zero(size_t m, size_t n) {
+		Matrix result(m, n);
+		result.setAll(0.0f);
+		return result;
+	}
+protected:
+	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
+	inline float *getData() { return _matrix.pData; }
+	inline const float *getData() const { return _matrix.pData; }
+	inline void setData(float *data) { _matrix.pData = data; }
+private:
+	arm_matrix_instance_f32 _matrix;
+};
+
+} // namespace math
diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp
new file mode 100644
index 000000000..7ea6496bb
--- /dev/null
+++ b/src/lib/mathlib/math/arm/Vector.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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 Vector.cpp
+ *
+ * math vector
+ */
+
+#include "Vector.hpp"
diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
new file mode 100644
index 000000000..4155800e8
--- /dev/null
+++ b/src/lib/mathlib/math/arm/Vector.hpp
@@ -0,0 +1,236 @@
+/****************************************************************************
+ *
+ *   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 Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+#include "../test/test.hpp"
+
+// arm specific
+#include "../../CMSIS/Include/arm_math.h"
+
+namespace math
+{
+
+class __EXPORT Vector
+{
+public:
+	// constructor
+	Vector(size_t rows) :
+		_rows(rows),
+		_data((float *)calloc(rows, sizeof(float))) {
+	}
+	Vector(size_t rows, const float *data) :
+		_rows(rows),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), data, getSize());
+	}
+	// deconstructor
+	virtual ~Vector() {
+		delete [] getData();
+	}
+	// copy constructor (deep)
+	Vector(const Vector &right) :
+		_rows(right.getRows()),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), right.getData(),
+		       right.getSize());
+	}
+	// assignment
+	inline Vector &operator=(const Vector &right) {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+
+		if (this != &right) {
+			memcpy(getData(), right.getData(),
+			       right.getSize());
+		}
+
+		return *this;
+	}
+	// element accessors
+	inline float &operator()(size_t i) {
+#ifdef VECTOR_ASSERT
+		ASSERT(i < getRows());
+#endif
+		return getData()[i];
+	}
+	inline const float &operator()(size_t i) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(i < getRows());
+#endif
+		return getData()[i];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			float sig;
+			int exp;
+			float num = (*this)(i);
+			float2SigExp(num, sig, exp);
+			printf("%6.3fe%03.3d,", (double)sig, exp);
+		}
+
+		printf("\n");
+	}
+	// boolean ops
+	inline bool operator==(const Vector &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
+				return false;
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Vector operator+(float right) const {
+		Vector result(getRows());
+		arm_offset_f32((float *)getData(),
+			       right, result.getData(),
+			       getRows());
+		return result;
+	}
+	inline Vector operator-(float right) const {
+		Vector result(getRows());
+		arm_offset_f32((float *)getData(),
+			       -right, result.getData(),
+			       getRows());
+		return result;
+	}
+	inline Vector operator*(float right) const {
+		Vector result(getRows());
+		arm_scale_f32((float *)getData(),
+			      right, result.getData(),
+			      getRows());
+		return result;
+	}
+	inline Vector operator/(float right) const {
+		Vector result(getRows());
+		arm_scale_f32((float *)getData(),
+			      1.0f / right, result.getData(),
+			      getRows());
+		return result;
+	}
+	// vector ops
+	inline Vector operator+(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+		Vector result(getRows());
+		arm_add_f32((float *)getData(),
+			    (float *)right.getData(),
+			    result.getData(),
+			    getRows());
+		return result;
+	}
+	inline Vector operator-(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+		Vector result(getRows());
+		arm_sub_f32((float *)getData(),
+			    (float *)right.getData(),
+			    result.getData(),
+			    getRows());
+		return result;
+	}
+	inline Vector operator-(void) const {
+		Vector result(getRows());
+		arm_negate_f32((float *)getData(),
+			    result.getData(),
+			    getRows());
+		return result;
+	}
+	// other functions
+	inline float dot(const Vector &right) const {
+		float result = 0;
+		arm_dot_prod_f32((float *)getData(),
+				 (float *)right.getData(),
+				 getRows(),
+				 &result);
+		return result;
+	}
+	inline float norm() const {
+		return sqrtf(dot(*this));
+	}
+	inline float length() const {
+		return norm();
+	}
+	inline Vector unit() const {
+		return (*this) / norm();
+	}
+	inline Vector normalized() const {
+		return unit();
+	}
+	inline void normalize() {
+		(*this) = (*this) / norm();
+	}
+	inline static Vector zero(size_t rows) {
+		Vector result(rows);
+		// calloc returns zeroed memory
+		return result;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			(*this)(i) = val;
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _rows; }
+	inline const float *getData() const { return _data; }
+protected:
+	inline size_t getSize() const { return sizeof(float) * getRows(); }
+	inline float *getData() { return _data; }
+	inline void setData(float *data) { _data = data; }
+private:
+	size_t _rows;
+	float *_data;
+};
+
+} // math
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
new file mode 100644
index 000000000..efb17225d
--- /dev/null
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -0,0 +1,77 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   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	LowPassFilter.cpp
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+
+#include "LowPassFilter2p.hpp"
+#include "math.h"
+
+namespace math
+{
+
+void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
+{
+    _cutoff_freq = cutoff_freq;
+    float fr = sample_freq/_cutoff_freq;
+    float ohm = tanf(M_PI_F/fr);
+    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
+    _b0 = ohm*ohm/c;
+    _b1 = 2.0f*_b0;
+    _b2 = _b0;
+    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
+    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
+}
+
+float LowPassFilter2p::apply(float sample)
+{
+    // do the filtering
+    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
+    if (isnan(delay_element_0) || isinf(delay_element_0)) {
+        // don't allow bad values to propogate via the filter
+        delay_element_0 = sample;
+    }
+    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+    
+    _delay_element_2 = _delay_element_1;
+    _delay_element_1 = delay_element_0;
+
+    // return the value.  Should be no need to check limits
+    return output;
+}
+
+} // namespace math
+
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
new file mode 100644
index 000000000..208ec98d4
--- /dev/null
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -0,0 +1,78 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   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	LowPassFilter.h
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+/// Adapted for PX4 by Andrew Tridgell
+
+#pragma once
+
+namespace math
+{
+class __EXPORT LowPassFilter2p
+{
+public:
+    // constructor
+    LowPassFilter2p(float sample_freq, float cutoff_freq) {
+        // set initial parameters
+        set_cutoff_frequency(sample_freq, cutoff_freq);
+        _delay_element_1 = _delay_element_2 = 0;
+    }
+
+    // change parameters
+    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
+
+    // apply - Add a new raw value to the filter 
+    // and retrieve the filtered result
+    float apply(float sample);
+
+    // return the cutoff frequency
+    float get_cutoff_freq(void) const {
+        return _cutoff_freq;
+    }
+
+private:
+    float           _cutoff_freq; 
+    float           _a1;
+    float           _a2;
+    float           _b0;
+    float           _b1;
+    float           _b2;
+    float           _delay_element_1;        // buffered sample -1
+    float           _delay_element_2;        // buffered sample -2
+};
+
+} // namespace math
diff --git a/src/lib/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk
new file mode 100644
index 000000000..fe92c8c70
--- /dev/null
+++ b/src/lib/mathlib/math/filter/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# filter library
+#
+SRCS		 = LowPassFilter2p.cpp
+
+#
+# In order to include .config we first have to save off the
+# current makefile name, since app.mk needs it.
+#
+APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
+-include $(TOPDIR)/.config
diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp
new file mode 100644
index 000000000..21661622a
--- /dev/null
+++ b/src/lib/mathlib/math/generic/Matrix.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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 Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include "Matrix.hpp"
diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp
new file mode 100644
index 000000000..5601a3447
--- /dev/null
+++ b/src/lib/mathlib/math/generic/Matrix.hpp
@@ -0,0 +1,437 @@
+/****************************************************************************
+ *
+ *   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 Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+#include "../Matrix.hpp"
+
+namespace math
+{
+
+class __EXPORT Matrix
+{
+public:
+	// constructor
+	Matrix(size_t rows, size_t cols) :
+		_rows(rows),
+		_cols(cols),
+		_data((float *)calloc(rows *cols, sizeof(float))) {
+	}
+	Matrix(size_t rows, size_t cols, const float *data) :
+		_rows(rows),
+		_cols(cols),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), data, getSize());
+	}
+	// deconstructor
+	virtual ~Matrix() {
+		delete [] getData();
+	}
+	// copy constructor (deep)
+	Matrix(const Matrix &right) :
+		_rows(right.getRows()),
+		_cols(right.getCols()),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), right.getData(),
+		       right.getSize());
+	}
+	// assignment
+	inline Matrix &operator=(const Matrix &right) {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+
+		if (this != &right) {
+			memcpy(getData(), right.getData(),
+			       right.getSize());
+		}
+
+		return *this;
+	}
+	// element accessors
+	inline float &operator()(size_t i, size_t j) {
+#ifdef MATRIX_ASSERT
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
+#endif
+		return getData()[i * getCols() + j];
+	}
+	inline const float &operator()(size_t i, size_t j) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
+#endif
+		return getData()[i * getCols() + j];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				float sig;
+				int exp;
+				float num = (*this)(i, j);
+				float2SigExp(num, sig, exp);
+				printf("%6.3fe%03.3d,", (double)sig, exp);
+			}
+
+			printf("\n");
+		}
+	}
+	// boolean ops
+	inline bool operator==(const Matrix &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
+					return false;
+			}
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Matrix operator+(const float &right) const {
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) + right;
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator-(const float &right) const {
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) - right;
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator*(const float &right) const {
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) * right;
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator/(const float &right) const {
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) / right;
+			}
+		}
+
+		return result;
+	}
+	// vector ops
+	inline Vector operator*(const Vector &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getCols() == right.getRows());
+#endif
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i) += (*this)(i, j) * right(j);
+			}
+		}
+
+		return result;
+	}
+	// matrix ops
+	inline Matrix operator+(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) + right(i, j);
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator-(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) - right(i, j);
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator*(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getCols() == right.getRows());
+#endif
+		Matrix result(getRows(), right.getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < right.getCols(); j++) {
+				for (size_t k = 0; k < right.getRows(); k++) {
+					result(i, j) += (*this)(i, k) * right(k, j);
+				}
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator/(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(right.getRows() == right.getCols());
+		ASSERT(getCols() == right.getCols());
+#endif
+		return (*this) * right.inverse();
+	}
+	// other functions
+	inline Matrix transpose() const {
+		Matrix result(getCols(), getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(j, i) = (*this)(i, j);
+			}
+		}
+
+		return result;
+	}
+	inline void swapRows(size_t a, size_t b) {
+		if (a == b) return;
+
+		for (size_t j = 0; j < getCols(); j++) {
+			float tmp = (*this)(a, j);
+			(*this)(a, j) = (*this)(b, j);
+			(*this)(b, j) = tmp;
+		}
+	}
+	inline void swapCols(size_t a, size_t b) {
+		if (a == b) return;
+
+		for (size_t i = 0; i < getRows(); i++) {
+			float tmp = (*this)(i, a);
+			(*this)(i, a) = (*this)(i, b);
+			(*this)(i, b) = tmp;
+		}
+	}
+	/**
+	 * inverse based on LU factorization with partial pivotting
+	 */
+	Matrix inverse() const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == getCols());
+#endif
+		size_t N = getRows();
+		Matrix L = identity(N);
+		const Matrix &A = (*this);
+		Matrix U = A;
+		Matrix P = identity(N);
+
+		//printf("A:\n"); A.print();
+
+		// for all diagonal elements
+		for (size_t n = 0; n < N; n++) {
+
+			// if diagonal is zero, swap with row below
+			if (fabsf(U(n, n)) < 1e-8f) {
+				//printf("trying pivot for row %d\n",n);
+				for (size_t i = 0; i < N; i++) {
+					if (i == n) continue;
+
+					//printf("\ttrying row %d\n",i);
+					if (fabsf(U(i, n)) > 1e-8f) {
+						//printf("swapped %d\n",i);
+						U.swapRows(i, n);
+						P.swapRows(i, n);
+					}
+				}
+			}
+
+#ifdef MATRIX_ASSERT
+			//printf("A:\n"); A.print();
+			//printf("U:\n"); U.print();
+			//printf("P:\n"); P.print();
+			//fflush(stdout);
+			ASSERT(fabsf(U(n, n)) > 1e-8f);
+#endif
+
+			// failsafe, return zero matrix
+			if (fabsf(U(n, n)) < 1e-8f) {
+				return Matrix::zero(n);
+			}
+
+			// for all rows below diagonal
+			for (size_t i = (n + 1); i < N; i++) {
+				L(i, n) = U(i, n) / U(n, n);
+
+				// add i-th row and n-th row
+				// multiplied by: -a(i,n)/a(n,n)
+				for (size_t k = n; k < N; k++) {
+					U(i, k) -= L(i, n) * U(n, k);
+				}
+			}
+		}
+
+		//printf("L:\n"); L.print();
+		//printf("U:\n"); U.print();
+
+		// solve LY=P*I for Y by forward subst
+		Matrix Y = P;
+
+		// for all columns of Y
+		for (size_t c = 0; c < N; c++) {
+			// for all rows of L
+			for (size_t i = 0; i < N; i++) {
+				// for all columns of L
+				for (size_t j = 0; j < i; j++) {
+					// for all existing y
+					// subtract the component they
+					// contribute to the solution
+					Y(i, c) -= L(i, j) * Y(j, c);
+				}
+
+				// divide by the factor
+				// on current
+				// term to be solved
+				// Y(i,c) /= L(i,i);
+				// but L(i,i) = 1.0
+			}
+		}
+
+		//printf("Y:\n"); Y.print();
+
+		// solve Ux=y for x by back subst
+		Matrix X = Y;
+
+		// for all columns of X
+		for (size_t c = 0; c < N; c++) {
+			// for all rows of U
+			for (size_t k = 0; k < N; k++) {
+				// have to go in reverse order
+				size_t i = N - 1 - k;
+
+				// for all columns of U
+				for (size_t j = i + 1; j < N; j++) {
+					// for all existing x
+					// subtract the component they
+					// contribute to the solution
+					X(i, c) -= U(i, j) * X(j, c);
+				}
+
+				// divide by the factor
+				// on current
+				// term to be solved
+				X(i, c) /= U(i, i);
+			}
+		}
+
+		//printf("X:\n"); X.print();
+		return X;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				(*this)(i, j) = val;
+			}
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _rows; }
+	inline size_t getCols() const { return _cols; }
+	inline static Matrix identity(size_t size) {
+		Matrix result(size, size);
+
+		for (size_t i = 0; i < size; i++) {
+			result(i, i) = 1.0f;
+		}
+
+		return result;
+	}
+	inline static Matrix zero(size_t size) {
+		Matrix result(size, size);
+		result.setAll(0.0f);
+		return result;
+	}
+	inline static Matrix zero(size_t m, size_t n) {
+		Matrix result(m, n);
+		result.setAll(0.0f);
+		return result;
+	}
+protected:
+	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
+	inline float *getData() { return _data; }
+	inline const float *getData() const { return _data; }
+	inline void setData(float *data) { _data = data; }
+private:
+	size_t _rows;
+	size_t _cols;
+	float *_data;
+};
+
+} // namespace math
diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp
new file mode 100644
index 000000000..7ea6496bb
--- /dev/null
+++ b/src/lib/mathlib/math/generic/Vector.cpp
@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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 Vector.cpp
+ *
+ * math vector
+ */
+
+#include "Vector.hpp"
diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp
new file mode 100644
index 000000000..8cfdc676d
--- /dev/null
+++ b/src/lib/mathlib/math/generic/Vector.hpp
@@ -0,0 +1,245 @@
+/****************************************************************************
+ *
+ *   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 Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+
+namespace math
+{
+
+class __EXPORT Vector
+{
+public:
+	// constructor
+	Vector(size_t rows) :
+		_rows(rows),
+		_data((float *)calloc(rows, sizeof(float))) {
+	}
+	Vector(size_t rows, const float *data) :
+		_rows(rows),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), data, getSize());
+	}
+	// deconstructor
+	virtual ~Vector() {
+		delete [] getData();
+	}
+	// copy constructor (deep)
+	Vector(const Vector &right) :
+		_rows(right.getRows()),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), right.getData(),
+		       right.getSize());
+	}
+	// assignment
+	inline Vector &operator=(const Vector &right) {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+
+		if (this != &right) {
+			memcpy(getData(), right.getData(),
+			       right.getSize());
+		}
+
+		return *this;
+	}
+	// element accessors
+	inline float &operator()(size_t i) {
+#ifdef VECTOR_ASSERT
+		ASSERT(i < getRows());
+#endif
+		return getData()[i];
+	}
+	inline const float &operator()(size_t i) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(i < getRows());
+#endif
+		return getData()[i];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			float sig;
+			int exp;
+			float num = (*this)(i);
+			float2SigExp(num, sig, exp);
+			printf("%6.3fe%03.3d,", (double)sig, exp);
+		}
+
+		printf("\n");
+	}
+	// boolean ops
+	inline bool operator==(const Vector &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
+				return false;
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Vector operator+(const float &right) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) + right;
+		}
+
+		return result;
+	}
+	inline Vector operator-(const float &right) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) - right;
+		}
+
+		return result;
+	}
+	inline Vector operator*(const float &right) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) * right;
+		}
+
+		return result;
+	}
+	inline Vector operator/(const float &right) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) / right;
+		}
+
+		return result;
+	}
+	// vector ops
+	inline Vector operator+(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) + right(i);
+		}
+
+		return result;
+	}
+	inline Vector operator-(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) - right(i);
+		}
+
+		return result;
+	}
+	inline Vector operator-(void) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = -((*this)(i));
+		}
+
+		return result;
+	}
+	// other functions
+	inline float dot(const Vector &right) const {
+		float result = 0;
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result += (*this)(i) * (*this)(i);
+		}
+
+		return result;
+	}
+	inline float norm() const {
+		return sqrtf(dot(*this));
+	}
+	inline float length() const {
+		return norm();
+	}
+	inline Vector unit() const {
+		return (*this) / norm();
+	}
+	inline Vector normalized() const {
+		return unit();
+	}
+	inline void normalize() {
+		(*this) = (*this) / norm();
+	}
+	inline static Vector zero(size_t rows) {
+		Vector result(rows);
+		// calloc returns zeroed memory
+		return result;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			(*this)(i) = val;
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _rows; }
+protected:
+	inline size_t getSize() const { return sizeof(float) * getRows(); }
+	inline float *getData() { return _data; }
+	inline const float *getData() const { return _data; }
+	inline void setData(float *data) { _data = data; }
+private:
+	size_t _rows;
+	float *_data;
+};
+
+} // math
diff --git a/src/lib/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf
new file mode 100644
index 000000000..eb67a4bfc
Binary files /dev/null and b/src/lib/mathlib/math/nasa_rotation_def.pdf differ
diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp
new file mode 100644
index 000000000..2fa2f7e7c
--- /dev/null
+++ b/src/lib/mathlib/math/test/test.cpp
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   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 test.cpp
+ *
+ * Test library code
+ */
+
+#include <stdio.h>
+#include <math.h>
+#include <stdlib.h>
+
+#include "test.hpp"
+
+bool __EXPORT equal(float a, float b, float epsilon)
+{
+	float diff = fabsf(a - b);
+
+	if (diff > epsilon) {
+		printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
+		return false;
+
+	} else return true;
+}
+
+void __EXPORT float2SigExp(
+	const float &num,
+	float &sig,
+	int &exp)
+{
+	if (isnan(num) || isinf(num)) {
+		sig = 0.0f;
+		exp = -99;
+		return;
+	}
+
+	if (fabsf(num) < 1.0e-38f) {
+		sig = 0;
+		exp = 0;
+		return;
+	}
+
+	exp = log10f(fabsf(num));
+
+	if (exp > 0) {
+		exp = ceil(exp);
+
+	} else {
+		exp = floor(exp);
+	}
+
+	sig = num;
+
+	// cheap power since it is integer
+	if (exp > 0) {
+		for (int i = 0; i < abs(exp); i++) sig /= 10;
+
+	} else {
+		for (int i = 0; i < abs(exp); i++) sig *= 10;
+	}
+}
+
+
diff --git a/src/lib/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp
new file mode 100644
index 000000000..2027bb827
--- /dev/null
+++ b/src/lib/mathlib/math/test/test.hpp
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   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 test.hpp
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+//#include <assert.h>
+//#include <time.h>
+//#include <stdlib.h>
+
+bool equal(float a, float b, float eps = 1e-5);
+void float2SigExp(
+	const float &num,
+	float &sig,
+	int &exp);
diff --git a/src/lib/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce
new file mode 100644
index 000000000..c3fba4729
--- /dev/null
+++ b/src/lib/mathlib/math/test_math.sce
@@ -0,0 +1,63 @@
+clc
+clear
+function out = float_truncate(in, digits)
+    out = round(in*10^digits)
+    out = out/10^digits
+endfunction
+
+phi = 0.1
+theta = 0.2
+psi = 0.3
+
+cosPhi = cos(phi)
+cosPhi_2 = cos(phi/2)
+sinPhi = sin(phi)
+sinPhi_2 = sin(phi/2)
+
+cosTheta = cos(theta)
+cosTheta_2 = cos(theta/2)
+sinTheta = sin(theta)
+sinTheta_2 = sin(theta/2)
+
+cosPsi = cos(psi)
+cosPsi_2 = cos(psi/2)
+sinPsi = sin(psi)
+sinPsi_2 = sin(psi/2)
+
+C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
+        cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
+        -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
+    
+disp(C_nb)
+//C_nb = float_truncate(C_nb,3)
+//disp(C_nb)
+
+theta = asin(-C_nb(3,1))
+phi = atan(C_nb(3,2), C_nb(3,3))
+psi = atan(C_nb(2,1), C_nb(1,1))
+printf('phi %f\n', phi)
+printf('theta %f\n', theta)
+printf('psi %f\n', psi)
+
+q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
+     sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
+     cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
+     cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
+     
+//q = float_truncate(q,3)
+
+a = q(1)
+b = q(2)
+c = q(3)
+d = q(4)
+printf('q: %f %f %f %f\n', a, b, c, d)
+a2 = a*a
+b2 = b*b
+c2 = c*c
+d2 = d*d
+
+C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
+         2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
+         2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
+
+disp(C2_nb)
diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h
new file mode 100644
index 000000000..40ffb22bc
--- /dev/null
+++ b/src/lib/mathlib/mathlib.h
@@ -0,0 +1,59 @@
+/****************************************************************************
+ *
+ *   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 mathlib.h
+ *
+ * Common header for mathlib exports.
+ */
+
+#ifdef __cplusplus
+
+#pragma once
+
+#include "math/Dcm.hpp"
+#include "math/EulerAngles.hpp"
+#include "math/Matrix.hpp"
+#include "math/Quaternion.hpp"
+#include "math/Vector.hpp"
+#include "math/Vector3.hpp"
+#include "math/Vector2f.hpp"
+#include "math/Limits.hpp"
+
+#endif
+
+#ifdef CONFIG_ARCH_ARM
+
+#include "CMSIS/Include/arm_math.h"
+
+#endif
\ No newline at end of file
diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk
new file mode 100644
index 000000000..2146a1413
--- /dev/null
+++ b/src/lib/mathlib/module.mk
@@ -0,0 +1,62 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Math library
+#
+SRCS		 = math/test/test.cpp \
+		   math/Vector.cpp \
+		   math/Vector2f.cpp \
+		   math/Vector3.cpp \
+		   math/EulerAngles.cpp \
+		   math/Quaternion.cpp \
+		   math/Dcm.cpp \
+		   math/Matrix.cpp \
+		   math/Limits.cpp
+
+#
+# In order to include .config we first have to save off the
+# current makefile name, since app.mk needs it.
+#
+APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
+-include $(TOPDIR)/.config
+
+ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
+INCLUDE_DIRS	+= math/arm
+SRCS		+= math/arm/Vector.cpp \
+		   math/arm/Matrix.cpp
+else
+#INCLUDE_DIRS	+= math/generic
+#SRCS		+= math/generic/Vector.cpp \
+#		   math/generic/Matrix.cpp
+endif
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index b6217a414..ddd2e23d9 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -116,7 +116,7 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_accel.h>
-#include <systemlib/conversions.h>
+#include <geo/geo.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 9c0720aa5..46dc1bec2 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -58,7 +58,7 @@
 #include <poll.h>
 
 extern "C" {
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 }
 
 #include "../blocks.hpp"
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
deleted file mode 100644
index 8f39acd9d..000000000
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
+++ /dev/null
@@ -1,264 +0,0 @@
-/**************************************************************************//**
- * @file     ARMCM3.h
- * @brief    CMSIS Core Peripheral Access Layer Header File for
- *           ARMCM3 Device Series
- * @version  V1.07
- * @date     30. January 2012
- *
- * @note
- * Copyright (C) 2012 ARM Limited. All rights reserved.
- *
- * @par
- * ARM Limited (ARM) is supplying this software for use with Cortex-M 
- * processor based microcontrollers.  This file can be freely distributed 
- * within development tools that are supporting such ARM based processors. 
- *
- * @par
- * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
- * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
- * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
- * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
- *
- ******************************************************************************/
-
-#ifndef ARMCM3_H
-#define ARMCM3_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-/* -------------------------  Interrupt Number Definition  ------------------------ */
-
-typedef enum IRQn
-{
-/* -------------------  Cortex-M3 Processor Exceptions Numbers  ------------------- */
-  NonMaskableInt_IRQn           = -14,      /*!<  2 Non Maskable Interrupt          */
-  HardFault_IRQn                = -13,      /*!<  3 HardFault Interrupt             */
-  MemoryManagement_IRQn         = -12,      /*!<  4 Memory Management Interrupt     */
-  BusFault_IRQn                 = -11,      /*!<  5 Bus Fault Interrupt             */
-  UsageFault_IRQn               = -10,      /*!<  6 Usage Fault Interrupt           */
-  SVCall_IRQn                   =  -5,      /*!< 11 SV Call Interrupt               */
-  DebugMonitor_IRQn             =  -4,      /*!< 12 Debug Monitor Interrupt         */
-  PendSV_IRQn                   =  -2,      /*!< 14 Pend SV Interrupt               */
-  SysTick_IRQn                  =  -1,      /*!< 15 System Tick Interrupt           */
-
-/* ----------------------  ARMCM3 Specific Interrupt Numbers  --------------------- */
-  WDT_IRQn                      =   0,      /*!< Watchdog Timer Interrupt           */
-  RTC_IRQn                      =   1,      /*!< Real Time Clock Interrupt          */
-  TIM0_IRQn                     =   2,      /*!< Timer0 / Timer1 Interrupt          */
-  TIM2_IRQn                     =   3,      /*!< Timer2 / Timer3 Interrupt          */
-  MCIA_IRQn                     =   4,      /*!< MCIa Interrupt                     */
-  MCIB_IRQn                     =   5,      /*!< MCIb Interrupt                     */
-  UART0_IRQn                    =   6,      /*!< UART0 Interrupt                    */
-  UART1_IRQn                    =   7,      /*!< UART1 Interrupt                    */
-  UART2_IRQn                    =   8,      /*!< UART2 Interrupt                    */
-  UART4_IRQn                    =   9,      /*!< UART4 Interrupt                    */
-  AACI_IRQn                     =  10,      /*!< AACI / AC97 Interrupt              */
-  CLCD_IRQn                     =  11,      /*!< CLCD Combined Interrupt            */
-  ENET_IRQn                     =  12,      /*!< Ethernet Interrupt                 */
-  USBDC_IRQn                    =  13,      /*!< USB Device Interrupt               */
-  USBHC_IRQn                    =  14,      /*!< USB Host Controller Interrupt      */
-  CHLCD_IRQn                    =  15,      /*!< Character LCD Interrupt            */
-  FLEXRAY_IRQn                  =  16,      /*!< Flexray Interrupt                  */
-  CAN_IRQn                      =  17,      /*!< CAN Interrupt                      */
-  LIN_IRQn                      =  18,      /*!< LIN Interrupt                      */
-  I2C_IRQn                      =  19,      /*!< I2C ADC/DAC Interrupt              */
-  CPU_CLCD_IRQn                 =  28,      /*!< CPU CLCD Combined Interrupt        */
-  UART3_IRQn                    =  30,      /*!< UART3 Interrupt                    */
-  SPI_IRQn                      =  31,      /*!< SPI Touchscreen Interrupt          */
-} IRQn_Type;
-
-
-/* ================================================================================ */
-/* ================      Processor and Core Peripheral Section     ================ */
-/* ================================================================================ */
-
-/* --------  Configuration of the Cortex-M4 Processor and Core Peripherals  ------- */
-#define __CM3_REV                 0x0201    /*!< Core revision r2p1                              */
-#define __MPU_PRESENT             1         /*!< MPU present or not                              */
-#define __NVIC_PRIO_BITS          3         /*!< Number of Bits used for Priority Levels         */
-#define __Vendor_SysTickConfig    0         /*!< Set to 1 if different SysTick Config is used    */
-
-#include <core_cm3.h>                       /* Processor and core peripherals                    */
-/* NuttX */
-//#include "system_ARMCM3.h"                  /* System Header                                     */
-
-
-/* ================================================================================ */
-/* ================       Device Specific Peripheral Section       ================ */
-/* ================================================================================ */
-
-/* -------------------  Start of section using anonymous unions  ------------------ */
-#if defined(__CC_ARM)
-  #pragma push
-  #pragma anon_unions
-#elif defined(__ICCARM__)
-  #pragma language=extended
-#elif defined(__GNUC__)
-  /* anonymous unions are enabled by default */
-#elif defined(__TMS470__)
-/* anonymous unions are enabled by default */
-#elif defined(__TASKING__)
-  #pragma warning 586
-#else
-  #warning Not supported compiler type
-#endif
-
-
-
-/* ================================================================================ */
-/* ================            CPU FPGA System (CPU_SYS)           ================ */
-/* ================================================================================ */
-typedef struct
-{
-  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
-  __IO uint32_t MEMCFG;           /* Offset: 0x004 (R/W)  Remap and Alias Memory Control */
-  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
-  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
-  __I  uint32_t TS;               /* Offset: 0x010 (R/ )  Touchscreen Register */
-  __IO uint32_t CTRL1;            /* Offset: 0x014 (R/W)  Misc Control Functions */
-       uint32_t RESERVED0[2];
-  __IO uint32_t CLKCFG;           /* Offset: 0x020 (R/W)  System Clock Configuration */
-  __IO uint32_t WSCFG;            /* Offset: 0x024 (R/W)  Flash Waitstate Configuration */
-  __IO uint32_t CPUCFG;           /* Offset: 0x028 (R/W)  Processor Configuration */
-       uint32_t RESERVED1[3];
-  __IO uint32_t BASE;             /* Offset: 0x038 (R/W)  ROM Table base Address */
-  __IO uint32_t ID2;              /* Offset: 0x03C (R/W)  Secondary Identification Register */
-} ARM_CPU_SYS_TypeDef;
-
-
-/* ================================================================================ */
-/* ================            DUT FPGA System (DUT_SYS)           ================ */
-/* ================================================================================ */
-typedef struct
-{
-  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
-  __IO uint32_t PERCFG;           /* Offset: 0x004 (R/W)  Peripheral Control Signals */
-  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
-  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
-  __IO uint32_t SEG7;             /* Offset: 0x010 (R/W)  7-segment LED Output States */
-  __I  uint32_t CNT25MHz;         /* Offset: 0x014 (R/ )  Freerunning counter incrementing at 25MHz */
-  __I  uint32_t CNT100Hz;         /* Offset: 0x018 (R/ )  Freerunning counter incrementing at 100Hz */
-} ARM_DUT_SYS_TypeDef;
-
-
-/* ================================================================================ */
-/* ================                   Timer (TIM)                  ================ */
-/* ================================================================================ */
-typedef struct
-{
-  __IO uint32_t Timer1Load;       /* Offset: 0x000 (R/W)  Timer 1 Load */
-  __I  uint32_t Timer1Value;      /* Offset: 0x004 (R/ )  Timer 1 Counter Current Value */
-  __IO uint32_t Timer1Control;    /* Offset: 0x008 (R/W)  Timer 1 Control */
-  __O  uint32_t Timer1IntClr;     /* Offset: 0x00C ( /W)  Timer 1 Interrupt Clear */
-  __I  uint32_t Timer1RIS;        /* Offset: 0x010 (R/ )  Timer 1 Raw Interrupt Status */
-  __I  uint32_t Timer1MIS;        /* Offset: 0x014 (R/ )  Timer 1 Masked Interrupt Status */
-  __IO uint32_t Timer1BGLoad;     /* Offset: 0x018 (R/W)  Background Load Register */
-       uint32_t RESERVED0[1];
-  __IO uint32_t Timer2Load;       /* Offset: 0x020 (R/W)  Timer 2 Load */
-  __I  uint32_t Timer2Value;      /* Offset: 0x024 (R/ )  Timer 2 Counter Current Value */
-  __IO uint32_t Timer2Control;    /* Offset: 0x028 (R/W)  Timer 2 Control */
-  __O  uint32_t Timer2IntClr;     /* Offset: 0x02C ( /W)  Timer 2 Interrupt Clear */
-  __I  uint32_t Timer2RIS;        /* Offset: 0x030 (R/ )  Timer 2 Raw Interrupt Status */
-  __I  uint32_t Timer2MIS;        /* Offset: 0x034 (R/ )  Timer 2 Masked Interrupt Status */
-  __IO uint32_t Timer2BGLoad;     /* Offset: 0x038 (R/W)  Background Load Register */
-} ARM_TIM_TypeDef;
-
-
-/* ================================================================================ */
-/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
-/* ================================================================================ */
-typedef struct
-{
-  __IO uint32_t DR;               /* Offset: 0x000 (R/W)  Data */ 
-  union {
-  __I  uint32_t RSR;              /* Offset: 0x000 (R/ )  Receive Status */
-  __O  uint32_t ECR;              /* Offset: 0x000 ( /W)  Error Clear */
-  };
-       uint32_t RESERVED0[4];
-  __IO uint32_t FR;               /* Offset: 0x018 (R/W)  Flags */
-       uint32_t RESERVED1[1];
-  __IO uint32_t ILPR;             /* Offset: 0x020 (R/W)  IrDA Low-power Counter */
-  __IO uint32_t IBRD;             /* Offset: 0x024 (R/W)  Interger Baud Rate */
-  __IO uint32_t FBRD;             /* Offset: 0x028 (R/W)  Fractional Baud Rate */
-  __IO uint32_t LCR_H;            /* Offset: 0x02C (R/W)  Line Control */
-  __IO uint32_t CR;               /* Offset: 0x030 (R/W)  Control */
-  __IO uint32_t IFLS;             /* Offset: 0x034 (R/W)  Interrupt FIFO Level Select */
-  __IO uint32_t IMSC;             /* Offset: 0x038 (R/W)  Interrupt Mask Set / Clear */
-  __IO uint32_t RIS;              /* Offset: 0x03C (R/W)  Raw Interrupt Status */
-  __IO uint32_t MIS;              /* Offset: 0x040 (R/W)  Masked Interrupt Status */
-  __O  uint32_t ICR;              /* Offset: 0x044 ( /W)  Interrupt Clear */
-  __IO uint32_t DMACR;            /* Offset: 0x048 (R/W)  DMA Control */
-} ARM_UART_TypeDef;
-
-
-/* --------------------  End of section using anonymous unions  ------------------- */
-#if defined(__CC_ARM)
-  #pragma pop
-#elif defined(__ICCARM__)
-  /* leave anonymous unions enabled */
-#elif defined(__GNUC__)
-  /* anonymous unions are enabled by default */
-#elif defined(__TMS470__)
-  /* anonymous unions are enabled by default */
-#elif defined(__TASKING__)
-  #pragma warning restore
-#else
-  #warning Not supported compiler type
-#endif
-
-
-
-
-/* ================================================================================ */
-/* ================              Peripheral memory map             ================ */
-/* ================================================================================ */
-/* --------------------------  CPU FPGA memory map  ------------------------------- */
-#define ARM_FLASH_BASE            (0x00000000UL)
-#define ARM_RAM_BASE              (0x20000000UL)
-#define ARM_RAM_FPGA_BASE         (0x1EFF0000UL)
-#define ARM_CPU_CFG_BASE          (0xDFFF0000UL)
-
-#define ARM_CPU_SYS_BASE          (ARM_CPU_CFG_BASE  + 0x00000)
-#define ARM_UART3_BASE            (ARM_CPU_CFG_BASE  + 0x05000)
-
-/* --------------------------  DUT FPGA memory map  ------------------------------- */
-#define ARM_APB_BASE              (0x40000000UL)
-#define ARM_AHB_BASE              (0x4FF00000UL)
-#define ARM_DMC_BASE              (0x60000000UL)
-#define ARM_SMC_BASE              (0xA0000000UL)
-
-#define ARM_TIM0_BASE             (ARM_APB_BASE      + 0x02000)
-#define ARM_TIM2_BASE             (ARM_APB_BASE      + 0x03000)
-#define ARM_DUT_SYS_BASE          (ARM_APB_BASE      + 0x04000)
-#define ARM_UART0_BASE            (ARM_APB_BASE      + 0x06000)
-#define ARM_UART1_BASE            (ARM_APB_BASE      + 0x07000)
-#define ARM_UART2_BASE            (ARM_APB_BASE      + 0x08000)
-#define ARM_UART4_BASE            (ARM_APB_BASE      + 0x09000)
-
-
-/* ================================================================================ */
-/* ================             Peripheral declaration             ================ */
-/* ================================================================================ */
-/* --------------------------  CPU FPGA Peripherals  ------------------------------ */
-#define ARM_CPU_SYS               ((ARM_CPU_SYS_TypeDef *)  ARM_CPU_SYS_BASE)
-#define ARM_UART3                 ((   ARM_UART_TypeDef *)    ARM_UART3_BASE)
-
-/* --------------------------  DUT FPGA Peripherals  ------------------------------ */
-#define ARM_DUT_SYS               ((ARM_DUT_SYS_TypeDef *)  ARM_DUT_SYS_BASE)
-#define ARM_TIM0                  ((    ARM_TIM_TypeDef *)     ARM_TIM0_BASE)
-#define ARM_TIM2                  ((    ARM_TIM_TypeDef *)     ARM_TIM2_BASE)
-#define ARM_UART0                 ((   ARM_UART_TypeDef *)    ARM_UART0_BASE)
-#define ARM_UART1                 ((   ARM_UART_TypeDef *)    ARM_UART1_BASE)
-#define ARM_UART2                 ((   ARM_UART_TypeDef *)    ARM_UART2_BASE)
-#define ARM_UART4                 ((   ARM_UART_TypeDef *)    ARM_UART4_BASE)
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif  /* ARMCM3_H */
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
deleted file mode 100644
index 181b7e433..000000000
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
+++ /dev/null
@@ -1,265 +0,0 @@
-/**************************************************************************//**
- * @file     ARMCM4.h
- * @brief    CMSIS Core Peripheral Access Layer Header File for
- *           ARMCM4 Device Series
- * @version  V1.07
- * @date     30. January 2012
- *
- * @note
- * Copyright (C) 2012 ARM Limited. All rights reserved.
- *
- * @par
- * ARM Limited (ARM) is supplying this software for use with Cortex-M 
- * processor based microcontrollers.  This file can be freely distributed 
- * within development tools that are supporting such ARM based processors. 
- *
- * @par
- * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
- * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
- * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
- * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
- *
- ******************************************************************************/
-
-#ifndef ARMCM4_H
-#define ARMCM4_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-/* -------------------------  Interrupt Number Definition  ------------------------ */
-
-typedef enum IRQn
-{
-/* -------------------  Cortex-M4 Processor Exceptions Numbers  ------------------- */
-  NonMaskableInt_IRQn           = -14,      /*!<  2 Non Maskable Interrupt          */
-  HardFault_IRQn                = -13,      /*!<  3 HardFault Interrupt             */
-  MemoryManagement_IRQn         = -12,      /*!<  4 Memory Management Interrupt     */
-  BusFault_IRQn                 = -11,      /*!<  5 Bus Fault Interrupt             */
-  UsageFault_IRQn               = -10,      /*!<  6 Usage Fault Interrupt           */
-  SVCall_IRQn                   =  -5,      /*!< 11 SV Call Interrupt               */
-  DebugMonitor_IRQn             =  -4,      /*!< 12 Debug Monitor Interrupt         */
-  PendSV_IRQn                   =  -2,      /*!< 14 Pend SV Interrupt               */
-  SysTick_IRQn                  =  -1,      /*!< 15 System Tick Interrupt           */
-
-/* ----------------------  ARMCM4 Specific Interrupt Numbers  --------------------- */
-  WDT_IRQn                      =   0,      /*!< Watchdog Timer Interrupt           */
-  RTC_IRQn                      =   1,      /*!< Real Time Clock Interrupt          */
-  TIM0_IRQn                     =   2,      /*!< Timer0 / Timer1 Interrupt          */
-  TIM2_IRQn                     =   3,      /*!< Timer2 / Timer3 Interrupt          */
-  MCIA_IRQn                     =   4,      /*!< MCIa Interrupt                     */
-  MCIB_IRQn                     =   5,      /*!< MCIb Interrupt                     */
-  UART0_IRQn                    =   6,      /*!< UART0 Interrupt                    */
-  UART1_IRQn                    =   7,      /*!< UART1 Interrupt                    */
-  UART2_IRQn                    =   8,      /*!< UART2 Interrupt                    */
-  UART4_IRQn                    =   9,      /*!< UART4 Interrupt                    */
-  AACI_IRQn                     =  10,      /*!< AACI / AC97 Interrupt              */
-  CLCD_IRQn                     =  11,      /*!< CLCD Combined Interrupt            */
-  ENET_IRQn                     =  12,      /*!< Ethernet Interrupt                 */
-  USBDC_IRQn                    =  13,      /*!< USB Device Interrupt               */
-  USBHC_IRQn                    =  14,      /*!< USB Host Controller Interrupt      */
-  CHLCD_IRQn                    =  15,      /*!< Character LCD Interrupt            */
-  FLEXRAY_IRQn                  =  16,      /*!< Flexray Interrupt                  */
-  CAN_IRQn                      =  17,      /*!< CAN Interrupt                      */
-  LIN_IRQn                      =  18,      /*!< LIN Interrupt                      */
-  I2C_IRQn                      =  19,      /*!< I2C ADC/DAC Interrupt              */
-  CPU_CLCD_IRQn                 =  28,      /*!< CPU CLCD Combined Interrupt        */
-  UART3_IRQn                    =  30,      /*!< UART3 Interrupt                    */
-  SPI_IRQn                      =  31,      /*!< SPI Touchscreen Interrupt          */
-} IRQn_Type;
-
-
-/* ================================================================================ */
-/* ================      Processor and Core Peripheral Section     ================ */
-/* ================================================================================ */
-
-/* --------  Configuration of the Cortex-M4 Processor and Core Peripherals  ------- */
-#define __CM4_REV                 0x0001    /*!< Core revision r0p1                              */
-#define __MPU_PRESENT             1         /*!< MPU present or not                              */
-#define __NVIC_PRIO_BITS          3         /*!< Number of Bits used for Priority Levels         */
-#define __Vendor_SysTickConfig    0         /*!< Set to 1 if different SysTick Config is used    */
-#define __FPU_PRESENT             1         /*!< FPU present or not                              */
-
-#include <core_cm4.h>                       /* Processor and core peripherals                    */
-/* NuttX */
-//#include "system_ARMCM4.h"                  /* System Header                                     */
-
-
-/* ================================================================================ */
-/* ================       Device Specific Peripheral Section       ================ */
-/* ================================================================================ */
-
-/* -------------------  Start of section using anonymous unions  ------------------ */
-#if defined(__CC_ARM)
-  #pragma push
-  #pragma anon_unions
-#elif defined(__ICCARM__)
-  #pragma language=extended
-#elif defined(__GNUC__)
-  /* anonymous unions are enabled by default */
-#elif defined(__TMS470__)
-/* anonymous unions are enabled by default */
-#elif defined(__TASKING__)
-  #pragma warning 586
-#else
-  #warning Not supported compiler type
-#endif
-
-
-
-/* ================================================================================ */
-/* ================            CPU FPGA System (CPU_SYS)           ================ */
-/* ================================================================================ */
-typedef struct
-{
-  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
-  __IO uint32_t MEMCFG;           /* Offset: 0x004 (R/W)  Remap and Alias Memory Control */
-  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
-  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
-  __I  uint32_t TS;               /* Offset: 0x010 (R/ )  Touchscreen Register */
-  __IO uint32_t CTRL1;            /* Offset: 0x014 (R/W)  Misc Control Functions */
-       uint32_t RESERVED0[2];
-  __IO uint32_t CLKCFG;           /* Offset: 0x020 (R/W)  System Clock Configuration */
-  __IO uint32_t WSCFG;            /* Offset: 0x024 (R/W)  Flash Waitstate Configuration */
-  __IO uint32_t CPUCFG;           /* Offset: 0x028 (R/W)  Processor Configuration */
-       uint32_t RESERVED1[3];
-  __IO uint32_t BASE;             /* Offset: 0x038 (R/W)  ROM Table base Address */
-  __IO uint32_t ID2;              /* Offset: 0x03C (R/W)  Secondary Identification Register */
-} ARM_CPU_SYS_TypeDef;
-
-
-/* ================================================================================ */
-/* ================            DUT FPGA System (DUT_SYS)           ================ */
-/* ================================================================================ */
-typedef struct
-{
-  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
-  __IO uint32_t PERCFG;           /* Offset: 0x004 (R/W)  Peripheral Control Signals */
-  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
-  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
-  __IO uint32_t SEG7;             /* Offset: 0x010 (R/W)  7-segment LED Output States */
-  __I  uint32_t CNT25MHz;         /* Offset: 0x014 (R/ )  Freerunning counter incrementing at 25MHz */
-  __I  uint32_t CNT100Hz;         /* Offset: 0x018 (R/ )  Freerunning counter incrementing at 100Hz */
-} ARM_DUT_SYS_TypeDef;
-
-
-/* ================================================================================ */
-/* ================                   Timer (TIM)                  ================ */
-/* ================================================================================ */
-typedef struct
-{
-  __IO uint32_t Timer1Load;       /* Offset: 0x000 (R/W)  Timer 1 Load */
-  __I  uint32_t Timer1Value;      /* Offset: 0x004 (R/ )  Timer 1 Counter Current Value */
-  __IO uint32_t Timer1Control;    /* Offset: 0x008 (R/W)  Timer 1 Control */
-  __O  uint32_t Timer1IntClr;     /* Offset: 0x00C ( /W)  Timer 1 Interrupt Clear */
-  __I  uint32_t Timer1RIS;        /* Offset: 0x010 (R/ )  Timer 1 Raw Interrupt Status */
-  __I  uint32_t Timer1MIS;        /* Offset: 0x014 (R/ )  Timer 1 Masked Interrupt Status */
-  __IO uint32_t Timer1BGLoad;     /* Offset: 0x018 (R/W)  Background Load Register */
-       uint32_t RESERVED0[1];
-  __IO uint32_t Timer2Load;       /* Offset: 0x020 (R/W)  Timer 2 Load */
-  __I  uint32_t Timer2Value;      /* Offset: 0x024 (R/ )  Timer 2 Counter Current Value */
-  __IO uint32_t Timer2Control;    /* Offset: 0x028 (R/W)  Timer 2 Control */
-  __O  uint32_t Timer2IntClr;     /* Offset: 0x02C ( /W)  Timer 2 Interrupt Clear */
-  __I  uint32_t Timer2RIS;        /* Offset: 0x030 (R/ )  Timer 2 Raw Interrupt Status */
-  __I  uint32_t Timer2MIS;        /* Offset: 0x034 (R/ )  Timer 2 Masked Interrupt Status */
-  __IO uint32_t Timer2BGLoad;     /* Offset: 0x038 (R/W)  Background Load Register */
-} ARM_TIM_TypeDef;
-
-
-/* ================================================================================ */
-/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
-/* ================================================================================ */
-typedef struct
-{
-  __IO uint32_t DR;               /* Offset: 0x000 (R/W)  Data */ 
-  union {
-  __I  uint32_t RSR;              /* Offset: 0x000 (R/ )  Receive Status */
-  __O  uint32_t ECR;              /* Offset: 0x000 ( /W)  Error Clear */
-  };
-       uint32_t RESERVED0[4];
-  __IO uint32_t FR;               /* Offset: 0x018 (R/W)  Flags */
-       uint32_t RESERVED1[1];
-  __IO uint32_t ILPR;             /* Offset: 0x020 (R/W)  IrDA Low-power Counter */
-  __IO uint32_t IBRD;             /* Offset: 0x024 (R/W)  Interger Baud Rate */
-  __IO uint32_t FBRD;             /* Offset: 0x028 (R/W)  Fractional Baud Rate */
-  __IO uint32_t LCR_H;            /* Offset: 0x02C (R/W)  Line Control */
-  __IO uint32_t CR;               /* Offset: 0x030 (R/W)  Control */
-  __IO uint32_t IFLS;             /* Offset: 0x034 (R/W)  Interrupt FIFO Level Select */
-  __IO uint32_t IMSC;             /* Offset: 0x038 (R/W)  Interrupt Mask Set / Clear */
-  __IO uint32_t RIS;              /* Offset: 0x03C (R/W)  Raw Interrupt Status */
-  __IO uint32_t MIS;              /* Offset: 0x040 (R/W)  Masked Interrupt Status */
-  __O  uint32_t ICR;              /* Offset: 0x044 ( /W)  Interrupt Clear */
-  __IO uint32_t DMACR;            /* Offset: 0x048 (R/W)  DMA Control */
-} ARM_UART_TypeDef;
-
-
-/* --------------------  End of section using anonymous unions  ------------------- */
-#if defined(__CC_ARM)
-  #pragma pop
-#elif defined(__ICCARM__)
-  /* leave anonymous unions enabled */
-#elif defined(__GNUC__)
-  /* anonymous unions are enabled by default */
-#elif defined(__TMS470__)
-  /* anonymous unions are enabled by default */
-#elif defined(__TASKING__)
-  #pragma warning restore
-#else
-  #warning Not supported compiler type
-#endif
-
-
-
-
-/* ================================================================================ */
-/* ================              Peripheral memory map             ================ */
-/* ================================================================================ */
-/* --------------------------  CPU FPGA memory map  ------------------------------- */
-#define ARM_FLASH_BASE            (0x00000000UL)
-#define ARM_RAM_BASE              (0x20000000UL)
-#define ARM_RAM_FPGA_BASE         (0x1EFF0000UL)
-#define ARM_CPU_CFG_BASE          (0xDFFF0000UL)
-
-#define ARM_CPU_SYS_BASE          (ARM_CPU_CFG_BASE  + 0x00000)
-#define ARM_UART3_BASE            (ARM_CPU_CFG_BASE  + 0x05000)
-
-/* --------------------------  DUT FPGA memory map  ------------------------------- */
-#define ARM_APB_BASE              (0x40000000UL)
-#define ARM_AHB_BASE              (0x4FF00000UL)
-#define ARM_DMC_BASE              (0x60000000UL)
-#define ARM_SMC_BASE              (0xA0000000UL)
-
-#define ARM_TIM0_BASE             (ARM_APB_BASE      + 0x02000)
-#define ARM_TIM2_BASE             (ARM_APB_BASE      + 0x03000)
-#define ARM_DUT_SYS_BASE          (ARM_APB_BASE      + 0x04000)
-#define ARM_UART0_BASE            (ARM_APB_BASE      + 0x06000)
-#define ARM_UART1_BASE            (ARM_APB_BASE      + 0x07000)
-#define ARM_UART2_BASE            (ARM_APB_BASE      + 0x08000)
-#define ARM_UART4_BASE            (ARM_APB_BASE      + 0x09000)
-
-
-/* ================================================================================ */
-/* ================             Peripheral declaration             ================ */
-/* ================================================================================ */
-/* --------------------------  CPU FPGA Peripherals  ------------------------------ */
-#define ARM_CPU_SYS               ((ARM_CPU_SYS_TypeDef *)  ARM_CPU_SYS_BASE)
-#define ARM_UART3                 ((   ARM_UART_TypeDef *)    ARM_UART3_BASE)
-
-/* --------------------------  DUT FPGA Peripherals  ------------------------------ */
-#define ARM_DUT_SYS               ((ARM_DUT_SYS_TypeDef *)  ARM_DUT_SYS_BASE)
-#define ARM_TIM0                  ((    ARM_TIM_TypeDef *)     ARM_TIM0_BASE)
-#define ARM_TIM2                  ((    ARM_TIM_TypeDef *)     ARM_TIM2_BASE)
-#define ARM_UART0                 ((   ARM_UART_TypeDef *)    ARM_UART0_BASE)
-#define ARM_UART1                 ((   ARM_UART_TypeDef *)    ARM_UART1_BASE)
-#define ARM_UART2                 ((   ARM_UART_TypeDef *)    ARM_UART2_BASE)
-#define ARM_UART4                 ((   ARM_UART_TypeDef *)    ARM_UART4_BASE)
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif  /* ARMCM4_H */
diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/modules/mathlib/CMSIS/Include/arm_common_tables.h
deleted file mode 100644
index 9c37ab4e5..000000000
--- a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/* ----------------------------------------------------------------------
-* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
-*
-* $Date:        17. January 2013
-* $Revision:    V1.4.1
-*
-* Project:      CMSIS DSP Library
-* Title:        arm_common_tables.h
-*
-* Description:  This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions
-*
-* Target Processor: Cortex-M4/Cortex-M3
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*   - Redistributions of source code must retain the above copyright
-*     notice, this list of conditions and the following disclaimer.
-*   - 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.
-*   - Neither the name of ARM LIMITED nor the names of its contributors
-*     may be used to endorse or promote products derived from this
-*     software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-* -------------------------------------------------------------------- */
-
-#ifndef _ARM_COMMON_TABLES_H
-#define _ARM_COMMON_TABLES_H
-
-#include "arm_math.h"
-
-extern const uint16_t armBitRevTable[1024];
-extern const q15_t armRecipTableQ15[64];
-extern const q31_t armRecipTableQ31[64];
-extern const q31_t realCoefAQ31[1024];
-extern const q31_t realCoefBQ31[1024];
-extern const float32_t twiddleCoef_16[32];
-extern const float32_t twiddleCoef_32[64];
-extern const float32_t twiddleCoef_64[128];
-extern const float32_t twiddleCoef_128[256];
-extern const float32_t twiddleCoef_256[512];
-extern const float32_t twiddleCoef_512[1024];
-extern const float32_t twiddleCoef_1024[2048];
-extern const float32_t twiddleCoef_2048[4096];
-extern const float32_t twiddleCoef_4096[8192];
-#define twiddleCoef twiddleCoef_4096
-extern const q31_t twiddleCoefQ31[6144];
-extern const q15_t twiddleCoefQ15[6144];
-extern const float32_t twiddleCoef_rfft_32[32];
-extern const float32_t twiddleCoef_rfft_64[64];
-extern const float32_t twiddleCoef_rfft_128[128];
-extern const float32_t twiddleCoef_rfft_256[256];
-extern const float32_t twiddleCoef_rfft_512[512];
-extern const float32_t twiddleCoef_rfft_1024[1024];
-extern const float32_t twiddleCoef_rfft_2048[2048];
-extern const float32_t twiddleCoef_rfft_4096[4096];
-
-
-#define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20  )
-#define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48  )
-#define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56  )
-#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 )
-#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 )
-#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 )
-#define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800)
-#define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808)
-#define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032)
-
-extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH];
-
-#endif /*  ARM_COMMON_TABLES_H */
diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/modules/mathlib/CMSIS/Include/arm_const_structs.h
deleted file mode 100644
index 406f737dc..000000000
--- a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/* ----------------------------------------------------------------------
-* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
-*
-* $Date:        17. January 2013
-* $Revision:    V1.4.1
-*
-* Project:      CMSIS DSP Library
-* Title:        arm_const_structs.h
-*
-* Description:  This file has constant structs that are initialized for
-*               user convenience.  For example, some can be given as
-*               arguments to the arm_cfft_f32() function.
-*
-* Target Processor: Cortex-M4/Cortex-M3
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*   - Redistributions of source code must retain the above copyright
-*     notice, this list of conditions and the following disclaimer.
-*   - 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.
-*   - Neither the name of ARM LIMITED nor the names of its contributors
-*     may be used to endorse or promote products derived from this
-*     software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-* -------------------------------------------------------------------- */
-
-#ifndef _ARM_CONST_STRUCTS_H
-#define _ARM_CONST_STRUCTS_H
-
-#include "arm_math.h"
-#include "arm_common_tables.h"
-
-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = {
-      16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH
-   };
-
-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = {
-      32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH
-   };
-
-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = {
-      64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH
-   };
-
-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = {
-      128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH
-   };
-
-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = {
-      256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH
-   };
-
-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = {
-      512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH
-   };
-
-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = {
-      1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH
-   };
-
-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = {
-      2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH
-   };
-
-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = {
-      4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH
-   };
-
-#endif
diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/modules/mathlib/CMSIS/Include/arm_math.h
deleted file mode 100644
index 6f66f9ee3..000000000
--- a/src/modules/mathlib/CMSIS/Include/arm_math.h
+++ /dev/null
@@ -1,7318 +0,0 @@
-/* ----------------------------------------------------------------------
-* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
-*
-* $Date:        17. January 2013
-* $Revision:    V1.4.1
-*
-* Project:      CMSIS DSP Library
-* Title:        arm_math.h
-*
-* Description:  Public header file for CMSIS DSP Library
-*
-* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*   - Redistributions of source code must retain the above copyright
-*     notice, this list of conditions and the following disclaimer.
-*   - 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.
-*   - Neither the name of ARM LIMITED 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.
- * -------------------------------------------------------------------- */
-
-/**
-   \mainpage CMSIS DSP Software Library
-   *
-   * <b>Introduction</b>
-   *
-   * This user manual describes the CMSIS DSP software library,
-   * a suite of common signal processing functions for use on Cortex-M processor based devices.
-   *
-   * The library is divided into a number of functions each covering a specific category:
-   * - Basic math functions
-   * - Fast math functions
-   * - Complex math functions
-   * - Filters
-   * - Matrix functions
-   * - Transforms
-   * - Motor control functions
-   * - Statistical functions
-   * - Support functions
-   * - Interpolation functions
-   *
-   * The library has separate functions for operating on 8-bit integers, 16-bit integers,
-   * 32-bit integer and 32-bit floating-point values.
-   *
-   * <b>Using the Library</b>
-   *
-   * The library installer contains prebuilt versions of the libraries in the <code>Lib</code> folder.
-   * - arm_cortexM4lf_math.lib (Little endian and Floating Point Unit on Cortex-M4)
-   * - arm_cortexM4bf_math.lib (Big endian and Floating Point Unit on Cortex-M4)
-   * - arm_cortexM4l_math.lib (Little endian on Cortex-M4)
-   * - arm_cortexM4b_math.lib (Big endian on Cortex-M4)
-   * - arm_cortexM3l_math.lib (Little endian on Cortex-M3)
-   * - arm_cortexM3b_math.lib (Big endian on Cortex-M3)
-   * - arm_cortexM0l_math.lib (Little endian on Cortex-M0)
-   * - arm_cortexM0b_math.lib (Big endian on Cortex-M3)
-   *
-   * The library functions are declared in the public file <code>arm_math.h</code> which is placed in the <code>Include</code> folder.
-   * Simply include this file and link the appropriate library in the application and begin calling the library functions. The Library supports single
-   * public header file <code> arm_math.h</code> for Cortex-M4/M3/M0 with little endian and big endian. Same header file will be used for floating point unit(FPU) variants.
-   * Define the appropriate pre processor MACRO ARM_MATH_CM4 or  ARM_MATH_CM3 or
-   * ARM_MATH_CM0 or ARM_MATH_CM0PLUS depending on the target processor in the application.
-   *
-   * <b>Examples</b>
-   *
-   * The library ships with a number of examples which demonstrate how to use the library functions.
-   *
-   * <b>Toolchain Support</b>
-   *
-   * The library has been developed and tested with MDK-ARM version 4.60.
-   * The library is being tested in GCC and IAR toolchains and updates on this activity will be made available shortly.
-   *
-   * <b>Building the Library</b>
-   *
-   * The library installer contains project files to re build libraries on MDK Tool chain in the <code>CMSIS\\DSP_Lib\\Source\\ARM</code> folder.
-   * - arm_cortexM0b_math.uvproj
-   * - arm_cortexM0l_math.uvproj
-   * - arm_cortexM3b_math.uvproj
-   * - arm_cortexM3l_math.uvproj
-   * - arm_cortexM4b_math.uvproj
-   * - arm_cortexM4l_math.uvproj
-   * - arm_cortexM4bf_math.uvproj
-   * - arm_cortexM4lf_math.uvproj
-   *
-   *
-   * The project can be built by opening the appropriate project in MDK-ARM 4.60 chain and defining the optional pre processor MACROs detailed above.
-   *
-   * <b>Pre-processor Macros</b>
-   *
-   * Each library project have differant pre-processor macros.
-   *
-   * - UNALIGNED_SUPPORT_DISABLE:
-   *
-   * Define macro UNALIGNED_SUPPORT_DISABLE, If the silicon does not support unaligned memory access
-   *
-   * - ARM_MATH_BIG_ENDIAN:
-   *
-   * Define macro ARM_MATH_BIG_ENDIAN to build the library for big endian targets. By default library builds for little endian targets.
-   *
-   * - ARM_MATH_MATRIX_CHECK:
-   *
-   * Define macro ARM_MATH_MATRIX_CHECK for checking on the input and output sizes of matrices
-   *
-   * - ARM_MATH_ROUNDING:
-   *
-   * Define macro ARM_MATH_ROUNDING for rounding on support functions
-   *
-   * - ARM_MATH_CMx:
-   *
-   * Define macro ARM_MATH_CM4 for building the library on Cortex-M4 target, ARM_MATH_CM3 for building library on Cortex-M3 target
-   * and ARM_MATH_CM0 for building library on cortex-M0 target, ARM_MATH_CM0PLUS for building library on cortex-M0+ target.
-   *
-   * - __FPU_PRESENT:
-   *
-   * Initialize macro __FPU_PRESENT = 1 when building on FPU supported Targets. Enable this macro for M4bf and M4lf libraries
-   *
-   * <b>Copyright Notice</b>
-   *
-   * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
-   */
-
-
-/**
- * @defgroup groupMath Basic Math Functions
- */
-
-/**
- * @defgroup groupFastMath Fast Math Functions
- * This set of functions provides a fast approximation to sine, cosine, and square root.
- * As compared to most of the other functions in the CMSIS math library, the fast math functions
- * operate on individual values and not arrays.
- * There are separate functions for Q15, Q31, and floating-point data.
- *
- */
-
-/**
- * @defgroup groupCmplxMath Complex Math Functions
- * This set of functions operates on complex data vectors.
- * The data in the complex arrays is stored in an interleaved fashion
- * (real, imag, real, imag, ...).
- * In the API functions, the number of samples in a complex array refers
- * to the number of complex values; the array contains twice this number of
- * real values.
- */
-
-/**
- * @defgroup groupFilters Filtering Functions
- */
-
-/**
- * @defgroup groupMatrix Matrix Functions
- *
- * This set of functions provides basic matrix math operations.
- * The functions operate on matrix data structures.  For example,
- * the type
- * definition for the floating-point matrix structure is shown
- * below:
- * <pre>
- *     typedef struct
- *     {
- *       uint16_t numRows;     // number of rows of the matrix.
- *       uint16_t numCols;     // number of columns of the matrix.
- *       float32_t *pData;     // points to the data of the matrix.
- *     } arm_matrix_instance_f32;
- * </pre>
- * There are similar definitions for Q15 and Q31 data types.
- *
- * The structure specifies the size of the matrix and then points to
- * an array of data.  The array is of size <code>numRows X numCols</code>
- * and the values are arranged in row order.  That is, the
- * matrix element (i, j) is stored at:
- * <pre>
- *     pData[i*numCols + j]
- * </pre>
- *
- * \par Init Functions
- * There is an associated initialization function for each type of matrix
- * data structure.
- * The initialization function sets the values of the internal structure fields.
- * Refer to the function <code>arm_mat_init_f32()</code>, <code>arm_mat_init_q31()</code>
- * and <code>arm_mat_init_q15()</code> for floating-point, Q31 and Q15 types,  respectively.
- *
- * \par
- * Use of the initialization function is optional. However, if initialization function is used
- * then the instance structure cannot be placed into a const data section.
- * To place the instance structure in a const data
- * section, manually initialize the data structure.  For example:
- * <pre>
- * <code>arm_matrix_instance_f32 S = {nRows, nColumns, pData};</code>
- * <code>arm_matrix_instance_q31 S = {nRows, nColumns, pData};</code>
- * <code>arm_matrix_instance_q15 S = {nRows, nColumns, pData};</code>
- * </pre>
- * where <code>nRows</code> specifies the number of rows, <code>nColumns</code>
- * specifies the number of columns, and <code>pData</code> points to the
- * data array.
- *
- * \par Size Checking
- * By default all of the matrix functions perform size checking on the input and
- * output matrices.  For example, the matrix addition function verifies that the
- * two input matrices and the output matrix all have the same number of rows and
- * columns.  If the size check fails the functions return:
- * <pre>
- *     ARM_MATH_SIZE_MISMATCH
- * </pre>
- * Otherwise the functions return
- * <pre>
- *     ARM_MATH_SUCCESS
- * </pre>
- * There is some overhead associated with this matrix size checking.
- * The matrix size checking is enabled via the \#define
- * <pre>
- *     ARM_MATH_MATRIX_CHECK
- * </pre>
- * within the library project settings.  By default this macro is defined
- * and size checking is enabled.  By changing the project settings and
- * undefining this macro size checking is eliminated and the functions
- * run a bit faster.  With size checking disabled the functions always
- * return <code>ARM_MATH_SUCCESS</code>.
- */
-
-/**
- * @defgroup groupTransforms Transform Functions
- */
-
-/**
- * @defgroup groupController Controller Functions
- */
-
-/**
- * @defgroup groupStats Statistics Functions
- */
-/**
- * @defgroup groupSupport Support Functions
- */
-
-/**
- * @defgroup groupInterpolation Interpolation Functions
- * These functions perform 1- and 2-dimensional interpolation of data.
- * Linear interpolation is used for 1-dimensional data and
- * bilinear interpolation is used for 2-dimensional data.
- */
-
-/**
- * @defgroup groupExamples Examples
- */
-#ifndef _ARM_MATH_H
-#define _ARM_MATH_H
-
-#define __CMSIS_GENERIC         /* disable NVIC and Systick functions */
-
-/* PX4 */
-#include <nuttx/config.h>
-#ifdef CONFIG_ARCH_CORTEXM4
-# define ARM_MATH_CM4 1
-#endif
-#ifdef CONFIG_ARCH_CORTEXM3
-# define ARM_MATH_CM3 1
-#endif
-#ifdef CONFIG_ARCH_FPU
-# define __FPU_PRESENT 1
-#endif
-
-#if defined (ARM_MATH_CM4)
-#include "core_cm4.h"
-#elif defined (ARM_MATH_CM3)
-#include "core_cm3.h"
-#elif defined (ARM_MATH_CM0)
-#include "core_cm0.h"
-#define ARM_MATH_CM0_FAMILY
-#elif defined (ARM_MATH_CM0PLUS)
-#include "core_cm0plus.h"
-#define ARM_MATH_CM0_FAMILY
-#else
-#include "ARMCM4.h"
-#warning "Define either ARM_MATH_CM4 OR ARM_MATH_CM3...By Default building on ARM_MATH_CM4....."
-#endif
-
-#undef  __CMSIS_GENERIC         /* enable NVIC and Systick functions */
-#include "string.h"
-#include "math.h"
-#ifdef	__cplusplus
-extern "C"
-{
-#endif
-
-
-  /**
-   * @brief Macros required for reciprocal calculation in Normalized LMS
-   */
-
-#define DELTA_Q31 			(0x100)
-#define DELTA_Q15 			0x5
-#define INDEX_MASK 			0x0000003F
-#ifndef PI
-#define PI					3.14159265358979f
-#endif
-
-  /**
-   * @brief Macros required for SINE and COSINE Fast math approximations
-   */
-
-#define TABLE_SIZE			256
-#define TABLE_SPACING_Q31	0x800000
-#define TABLE_SPACING_Q15	0x80
-
-  /**
-   * @brief Macros required for SINE and COSINE Controller functions
-   */
-  /* 1.31(q31) Fixed value of 2/360 */
-  /* -1 to +1 is divided into 360 values so total spacing is (2/360) */
-#define INPUT_SPACING			0xB60B61
-
-  /**
-   * @brief Macro for Unaligned Support
-   */
-#ifndef UNALIGNED_SUPPORT_DISABLE
-    #define ALIGN4
-#else
-  #if defined  (__GNUC__)
-    #define ALIGN4 __attribute__((aligned(4)))
-  #else
-    #define ALIGN4 __align(4)
-  #endif
-#endif	/*	#ifndef UNALIGNED_SUPPORT_DISABLE	*/
-
-  /**
-   * @brief Error status returned by some functions in the library.
-   */
-
-  typedef enum
-  {
-    ARM_MATH_SUCCESS = 0,                /**< No error */
-    ARM_MATH_ARGUMENT_ERROR = -1,        /**< One or more arguments are incorrect */
-    ARM_MATH_LENGTH_ERROR = -2,          /**< Length of data buffer is incorrect */
-    ARM_MATH_SIZE_MISMATCH = -3,         /**< Size of matrices is not compatible with the operation. */
-    ARM_MATH_NANINF = -4,                /**< Not-a-number (NaN) or infinity is generated */
-    ARM_MATH_SINGULAR = -5,              /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */
-    ARM_MATH_TEST_FAILURE = -6           /**< Test Failed  */
-  } arm_status;
-
-  /**
-   * @brief 8-bit fractional data type in 1.7 format.
-   */
-  typedef int8_t q7_t;
-
-  /**
-   * @brief 16-bit fractional data type in 1.15 format.
-   */
-  typedef int16_t q15_t;
-
-  /**
-   * @brief 32-bit fractional data type in 1.31 format.
-   */
-  typedef int32_t q31_t;
-
-  /**
-   * @brief 64-bit fractional data type in 1.63 format.
-   */
-  typedef int64_t q63_t;
-
-  /**
-   * @brief 32-bit floating-point type definition.
-   */
-  typedef float float32_t;
-
-  /**
-   * @brief 64-bit floating-point type definition.
-   */
-  typedef double float64_t;
-
-  /**
-   * @brief definition to read/write two 16 bit values.
-   */
-#if defined __CC_ARM
-#define __SIMD32_TYPE int32_t __packed
-#define CMSIS_UNUSED __attribute__((unused))
-#elif defined __ICCARM__
-#define CMSIS_UNUSED
-#define __SIMD32_TYPE int32_t __packed
-#elif defined __GNUC__
-#define __SIMD32_TYPE int32_t
-#define CMSIS_UNUSED __attribute__((unused))
-#else
-#error Unknown compiler
-#endif
-
-#define __SIMD32(addr)  (*(__SIMD32_TYPE **) & (addr))
-#define __SIMD32_CONST(addr)  ((__SIMD32_TYPE *)(addr))
-
-#define _SIMD32_OFFSET(addr)  (*(__SIMD32_TYPE *)  (addr))
-
-#define __SIMD64(addr)  (*(int64_t **) & (addr))
-
-#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)
-  /**
-   * @brief definition to pack two 16 bit values.
-   */
-#define __PKHBT(ARG1, ARG2, ARG3)      ( (((int32_t)(ARG1) <<  0) & (int32_t)0x0000FFFF) | \
-                                         (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000)  )
-#define __PKHTB(ARG1, ARG2, ARG3)      ( (((int32_t)(ARG1) <<  0) & (int32_t)0xFFFF0000) | \
-                                         (((int32_t)(ARG2) >> ARG3) & (int32_t)0x0000FFFF)  )
-
-#endif
-
-
-   /**
-   * @brief definition to pack four 8 bit values.
-   */
-#ifndef ARM_MATH_BIG_ENDIAN
-
-#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) <<  0) & (int32_t)0x000000FF) |	\
-                                (((int32_t)(v1) <<  8) & (int32_t)0x0000FF00) |	\
-							    (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) |	\
-							    (((int32_t)(v3) << 24) & (int32_t)0xFF000000)  )
-#else
-
-#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) <<  0) & (int32_t)0x000000FF) |	\
-                                (((int32_t)(v2) <<  8) & (int32_t)0x0000FF00) |	\
-							    (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) |	\
-							    (((int32_t)(v0) << 24) & (int32_t)0xFF000000)  )
-
-#endif
-
-
-  /**
-   * @brief Clips Q63 to Q31 values.
-   */
-  static __INLINE q31_t clip_q63_to_q31(
-  q63_t x)
-  {
-    return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
-      ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x;
-  }
-
-  /**
-   * @brief Clips Q63 to Q15 values.
-   */
-  static __INLINE q15_t clip_q63_to_q15(
-  q63_t x)
-  {
-    return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
-      ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15);
-  }
-
-  /**
-   * @brief Clips Q31 to Q7 values.
-   */
-  static __INLINE q7_t clip_q31_to_q7(
-  q31_t x)
-  {
-    return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ?
-      ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x;
-  }
-
-  /**
-   * @brief Clips Q31 to Q15 values.
-   */
-  static __INLINE q15_t clip_q31_to_q15(
-  q31_t x)
-  {
-    return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ?
-      ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x;
-  }
-
-  /**
-   * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format.
-   */
-
-  static __INLINE q63_t mult32x64(
-  q63_t x,
-  q31_t y)
-  {
-    return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) +
-            (((q63_t) (x >> 32) * y)));
-  }
-
-
-#if defined (ARM_MATH_CM0_FAMILY) && defined ( __CC_ARM   )
-#define __CLZ __clz
-#endif
-
-#if defined (ARM_MATH_CM0_FAMILY) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) )
-
-  static __INLINE uint32_t __CLZ(
-  q31_t data);
-
-
-  static __INLINE uint32_t __CLZ(
-  q31_t data)
-  {
-    uint32_t count = 0;
-    uint32_t mask = 0x80000000;
-
-    while((data & mask) == 0)
-    {
-      count += 1u;
-      mask = mask >> 1u;
-    }
-
-    return (count);
-
-  }
-
-#endif
-
-  /**
-   * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type.
-   */
-
-  static __INLINE uint32_t arm_recip_q31(
-  q31_t in,
-  q31_t * dst,
-  q31_t * pRecipTable)
-  {
-
-    uint32_t out, tempVal;
-    uint32_t index, i;
-    uint32_t signBits;
-
-    if(in > 0)
-    {
-      signBits = __CLZ(in) - 1;
-    }
-    else
-    {
-      signBits = __CLZ(-in) - 1;
-    }
-
-    /* Convert input sample to 1.31 format */
-    in = in << signBits;
-
-    /* calculation of index for initial approximated Val */
-    index = (uint32_t) (in >> 24u);
-    index = (index & INDEX_MASK);
-
-    /* 1.31 with exp 1 */
-    out = pRecipTable[index];
-
-    /* calculation of reciprocal value */
-    /* running approximation for two iterations */
-    for (i = 0u; i < 2u; i++)
-    {
-      tempVal = (q31_t) (((q63_t) in * out) >> 31u);
-      tempVal = 0x7FFFFFFF - tempVal;
-      /*      1.31 with exp 1 */
-      //out = (q31_t) (((q63_t) out * tempVal) >> 30u);
-      out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u);
-    }
-
-    /* write output */
-    *dst = out;
-
-    /* return num of signbits of out = 1/in value */
-    return (signBits + 1u);
-
-  }
-
-  /**
-   * @brief Function to Calculates 1/in (reciprocal) value of Q15 Data type.
-   */
-  static __INLINE uint32_t arm_recip_q15(
-  q15_t in,
-  q15_t * dst,
-  q15_t * pRecipTable)
-  {
-
-    uint32_t out = 0, tempVal = 0;
-    uint32_t index = 0, i = 0;
-    uint32_t signBits = 0;
-
-    if(in > 0)
-    {
-      signBits = __CLZ(in) - 17;
-    }
-    else
-    {
-      signBits = __CLZ(-in) - 17;
-    }
-
-    /* Convert input sample to 1.15 format */
-    in = in << signBits;
-
-    /* calculation of index for initial approximated Val */
-    index = in >> 8;
-    index = (index & INDEX_MASK);
-
-    /*      1.15 with exp 1  */
-    out = pRecipTable[index];
-
-    /* calculation of reciprocal value */
-    /* running approximation for two iterations */
-    for (i = 0; i < 2; i++)
-    {
-      tempVal = (q15_t) (((q31_t) in * out) >> 15);
-      tempVal = 0x7FFF - tempVal;
-      /*      1.15 with exp 1 */
-      out = (q15_t) (((q31_t) out * tempVal) >> 14);
-    }
-
-    /* write output */
-    *dst = out;
-
-    /* return num of signbits of out = 1/in value */
-    return (signBits + 1);
-
-  }
-
-
-  /*
-   * @brief C custom defined intrinisic function for only M0 processors
-   */
-#if defined(ARM_MATH_CM0_FAMILY)
-
-  static __INLINE q31_t __SSAT(
-  q31_t x,
-  uint32_t y)
-  {
-    int32_t posMax, negMin;
-    uint32_t i;
-
-    posMax = 1;
-    for (i = 0; i < (y - 1); i++)
-    {
-      posMax = posMax * 2;
-    }
-
-    if(x > 0)
-    {
-      posMax = (posMax - 1);
-
-      if(x > posMax)
-      {
-        x = posMax;
-      }
-    }
-    else
-    {
-      negMin = -posMax;
-
-      if(x < negMin)
-      {
-        x = negMin;
-      }
-    }
-    return (x);
-
-
-  }
-
-#endif /* end of ARM_MATH_CM0_FAMILY */
-
-
-
-  /*
-   * @brief C custom defined intrinsic function for M3 and M0 processors
-   */
-#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)
-
-  /*
-   * @brief C custom defined QADD8 for M3 and M0 processors
-   */
-  static __INLINE q31_t __QADD8(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t sum;
-    q7_t r, s, t, u;
-
-    r = (q7_t) x;
-    s = (q7_t) y;
-
-    r = __SSAT((q31_t) (r + s), 8);
-    s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8);
-    t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8);
-    u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8);
-
-    sum =
-      (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) |
-      (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF);
-
-    return sum;
-
-  }
-
-  /*
-   * @brief C custom defined QSUB8 for M3 and M0 processors
-   */
-  static __INLINE q31_t __QSUB8(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t sum;
-    q31_t r, s, t, u;
-
-    r = (q7_t) x;
-    s = (q7_t) y;
-
-    r = __SSAT((r - s), 8);
-    s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8;
-    t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16;
-    u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24;
-
-    sum =
-      (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r &
-                                                                0x000000FF);
-
-    return sum;
-  }
-
-  /*
-   * @brief C custom defined QADD16 for M3 and M0 processors
-   */
-
-  /*
-   * @brief C custom defined QADD16 for M3 and M0 processors
-   */
-  static __INLINE q31_t __QADD16(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t sum;
-    q31_t r, s;
-
-    r = (short) x;
-    s = (short) y;
-
-    r = __SSAT(r + s, 16);
-    s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16;
-
-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
-    return sum;
-
-  }
-
-  /*
-   * @brief C custom defined SHADD16 for M3 and M0 processors
-   */
-  static __INLINE q31_t __SHADD16(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t sum;
-    q31_t r, s;
-
-    r = (short) x;
-    s = (short) y;
-
-    r = ((r >> 1) + (s >> 1));
-    s = ((q31_t) ((x >> 17) + (y >> 17))) << 16;
-
-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
-    return sum;
-
-  }
-
-  /*
-   * @brief C custom defined QSUB16 for M3 and M0 processors
-   */
-  static __INLINE q31_t __QSUB16(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t sum;
-    q31_t r, s;
-
-    r = (short) x;
-    s = (short) y;
-
-    r = __SSAT(r - s, 16);
-    s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16;
-
-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
-    return sum;
-  }
-
-  /*
-   * @brief C custom defined SHSUB16 for M3 and M0 processors
-   */
-  static __INLINE q31_t __SHSUB16(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t diff;
-    q31_t r, s;
-
-    r = (short) x;
-    s = (short) y;
-
-    r = ((r >> 1) - (s >> 1));
-    s = (((x >> 17) - (y >> 17)) << 16);
-
-    diff = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
-    return diff;
-  }
-
-  /*
-   * @brief C custom defined QASX for M3 and M0 processors
-   */
-  static __INLINE q31_t __QASX(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t sum = 0;
-
-    sum =
-      ((sum +
-        clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) +
-      clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16)));
-
-    return sum;
-  }
-
-  /*
-   * @brief C custom defined SHASX for M3 and M0 processors
-   */
-  static __INLINE q31_t __SHASX(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t sum;
-    q31_t r, s;
-
-    r = (short) x;
-    s = (short) y;
-
-    r = ((r >> 1) - (y >> 17));
-    s = (((x >> 17) + (s >> 1)) << 16);
-
-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
-    return sum;
-  }
-
-
-  /*
-   * @brief C custom defined QSAX for M3 and M0 processors
-   */
-  static __INLINE q31_t __QSAX(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t sum = 0;
-
-    sum =
-      ((sum +
-        clip_q31_to_q15((q31_t) ((short) (x >> 16) - (short) y))) << 16) +
-      clip_q31_to_q15((q31_t) ((short) x + (short) (y >> 16)));
-
-    return sum;
-  }
-
-  /*
-   * @brief C custom defined SHSAX for M3 and M0 processors
-   */
-  static __INLINE q31_t __SHSAX(
-  q31_t x,
-  q31_t y)
-  {
-
-    q31_t sum;
-    q31_t r, s;
-
-    r = (short) x;
-    s = (short) y;
-
-    r = ((r >> 1) + (y >> 17));
-    s = (((x >> 17) - (s >> 1)) << 16);
-
-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
-    return sum;
-  }
-
-  /*
-   * @brief C custom defined SMUSDX for M3 and M0 processors
-   */
-  static __INLINE q31_t __SMUSDX(
-  q31_t x,
-  q31_t y)
-  {
-
-    return ((q31_t) (((short) x * (short) (y >> 16)) -
-                     ((short) (x >> 16) * (short) y)));
-  }
-
-  /*
-   * @brief C custom defined SMUADX for M3 and M0 processors
-   */
-  static __INLINE q31_t __SMUADX(
-  q31_t x,
-  q31_t y)
-  {
-
-    return ((q31_t) (((short) x * (short) (y >> 16)) +
-                     ((short) (x >> 16) * (short) y)));
-  }
-
-  /*
-   * @brief C custom defined QADD for M3 and M0 processors
-   */
-  static __INLINE q31_t __QADD(
-  q31_t x,
-  q31_t y)
-  {
-    return clip_q63_to_q31((q63_t) x + y);
-  }
-
-  /*
-   * @brief C custom defined QSUB for M3 and M0 processors
-   */
-  static __INLINE q31_t __QSUB(
-  q31_t x,
-  q31_t y)
-  {
-    return clip_q63_to_q31((q63_t) x - y);
-  }
-
-  /*
-   * @brief C custom defined SMLAD for M3 and M0 processors
-   */
-  static __INLINE q31_t __SMLAD(
-  q31_t x,
-  q31_t y,
-  q31_t sum)
-  {
-
-    return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
-            ((short) x * (short) y));
-  }
-
-  /*
-   * @brief C custom defined SMLADX for M3 and M0 processors
-   */
-  static __INLINE q31_t __SMLADX(
-  q31_t x,
-  q31_t y,
-  q31_t sum)
-  {
-
-    return (sum + ((short) (x >> 16) * (short) (y)) +
-            ((short) x * (short) (y >> 16)));
-  }
-
-  /*
-   * @brief C custom defined SMLSDX for M3 and M0 processors
-   */
-  static __INLINE q31_t __SMLSDX(
-  q31_t x,
-  q31_t y,
-  q31_t sum)
-  {
-
-    return (sum - ((short) (x >> 16) * (short) (y)) +
-            ((short) x * (short) (y >> 16)));
-  }
-
-  /*
-   * @brief C custom defined SMLALD for M3 and M0 processors
-   */
-  static __INLINE q63_t __SMLALD(
-  q31_t x,
-  q31_t y,
-  q63_t sum)
-  {
-
-    return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
-            ((short) x * (short) y));
-  }
-
-  /*
-   * @brief C custom defined SMLALDX for M3 and M0 processors
-   */
-  static __INLINE q63_t __SMLALDX(
-  q31_t x,
-  q31_t y,
-  q63_t sum)
-  {
-
-    return (sum + ((short) (x >> 16) * (short) y)) +
-      ((short) x * (short) (y >> 16));
-  }
-
-  /*
-   * @brief C custom defined SMUAD for M3 and M0 processors
-   */
-  static __INLINE q31_t __SMUAD(
-  q31_t x,
-  q31_t y)
-  {
-
-    return (((x >> 16) * (y >> 16)) +
-            (((x << 16) >> 16) * ((y << 16) >> 16)));
-  }
-
-  /*
-   * @brief C custom defined SMUSD for M3 and M0 processors
-   */
-  static __INLINE q31_t __SMUSD(
-  q31_t x,
-  q31_t y)
-  {
-
-    return (-((x >> 16) * (y >> 16)) +
-            (((x << 16) >> 16) * ((y << 16) >> 16)));
-  }
-
-
-  /*
-   * @brief C custom defined SXTB16 for M3 and M0 processors
-   */
-  static __INLINE q31_t __SXTB16(
-  q31_t x)
-  {
-
-    return ((((x << 24) >> 24) & 0x0000FFFF) |
-            (((x << 8) >> 8) & 0xFFFF0000));
-  }
-
-
-#endif /* defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) */
-
-
-  /**
-   * @brief Instance structure for the Q7 FIR filter.
-   */
-  typedef struct
-  {
-    uint16_t numTaps;        /**< number of filter coefficients in the filter. */
-    q7_t *pState;            /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    q7_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps.*/
-  } arm_fir_instance_q7;
-
-  /**
-   * @brief Instance structure for the Q15 FIR filter.
-   */
-  typedef struct
-  {
-    uint16_t numTaps;         /**< number of filter coefficients in the filter. */
-    q15_t *pState;            /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    q15_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps.*/
-  } arm_fir_instance_q15;
-
-  /**
-   * @brief Instance structure for the Q31 FIR filter.
-   */
-  typedef struct
-  {
-    uint16_t numTaps;         /**< number of filter coefficients in the filter. */
-    q31_t *pState;            /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    q31_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps. */
-  } arm_fir_instance_q31;
-
-  /**
-   * @brief Instance structure for the floating-point FIR filter.
-   */
-  typedef struct
-  {
-    uint16_t numTaps;     /**< number of filter coefficients in the filter. */
-    float32_t *pState;    /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    float32_t *pCoeffs;   /**< points to the coefficient array. The array is of length numTaps. */
-  } arm_fir_instance_f32;
-
-
-  /**
-   * @brief Processing function for the Q7 FIR filter.
-   * @param[in] *S points to an instance of the Q7 FIR filter 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.
-   * @return none.
-   */
-  void arm_fir_q7(
-  const arm_fir_instance_q7 * S,
-  q7_t * pSrc,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief  Initialization function for the Q7 FIR filter.
-   * @param[in,out] *S points to an instance of the Q7 FIR structure.
-   * @param[in] numTaps  Number of filter coefficients in the filter.
-   * @param[in] *pCoeffs points to the filter coefficients.
-   * @param[in] *pState points to the state buffer.
-   * @param[in] blockSize number of samples that are processed.
-   * @return none
-   */
-  void arm_fir_init_q7(
-  arm_fir_instance_q7 * S,
-  uint16_t numTaps,
-  q7_t * pCoeffs,
-  q7_t * pState,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Processing function for the Q15 FIR filter.
-   * @param[in] *S points to an instance of the Q15 FIR 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.
-   * @return none.
-   */
-  void arm_fir_q15(
-  const arm_fir_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the fast Q15 FIR filter for Cortex-M3 and Cortex-M4.
-   * @param[in] *S points to an instance of the Q15 FIR filter 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.
-   * @return none.
-   */
-  void arm_fir_fast_q15(
-  const arm_fir_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the Q15 FIR filter.
-   * @param[in,out] *S points to an instance of the Q15 FIR filter structure.
-   * @param[in] numTaps  Number of filter coefficients in the filter. Must be even and greater than or equal to 4.
-   * @param[in] *pCoeffs points to the filter coefficients.
-   * @param[in] *pState points to the state buffer.
-   * @param[in] blockSize number of samples that are processed at a time.
-   * @return The function returns ARM_MATH_SUCCESS if initialization was successful or ARM_MATH_ARGUMENT_ERROR if
-   * <code>numTaps</code> is not a supported value.
-   */
-
-  arm_status arm_fir_init_q15(
-  arm_fir_instance_q15 * S,
-  uint16_t numTaps,
-  q15_t * pCoeffs,
-  q15_t * pState,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the Q31 FIR filter.
-   * @param[in] *S points to an instance of the Q31 FIR filter 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.
-   * @return none.
-   */
-  void arm_fir_q31(
-  const arm_fir_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the fast Q31 FIR filter for Cortex-M3 and Cortex-M4.
-   * @param[in] *S points to an instance of the Q31 FIR 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.
-   * @return none.
-   */
-  void arm_fir_fast_q31(
-  const arm_fir_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the Q31 FIR filter.
-   * @param[in,out] *S points to an instance of the Q31 FIR structure.
-   * @param[in] 	numTaps  Number of filter coefficients in the filter.
-   * @param[in] 	*pCoeffs points to the filter coefficients.
-   * @param[in] 	*pState points to the state buffer.
-   * @param[in] 	blockSize number of samples that are processed at a time.
-   * @return 		none.
-   */
-  void arm_fir_init_q31(
-  arm_fir_instance_q31 * S,
-  uint16_t numTaps,
-  q31_t * pCoeffs,
-  q31_t * pState,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the floating-point FIR filter.
-   * @param[in] *S points to an instance of the floating-point FIR 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.
-   * @return none.
-   */
-  void arm_fir_f32(
-  const arm_fir_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the floating-point FIR filter.
-   * @param[in,out] *S points to an instance of the floating-point FIR filter structure.
-   * @param[in] 	numTaps  Number of filter coefficients in the filter.
-   * @param[in] 	*pCoeffs points to the filter coefficients.
-   * @param[in] 	*pState points to the state buffer.
-   * @param[in] 	blockSize number of samples that are processed at a time.
-   * @return    	none.
-   */
-  void arm_fir_init_f32(
-  arm_fir_instance_f32 * S,
-  uint16_t numTaps,
-  float32_t * pCoeffs,
-  float32_t * pState,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Instance structure for the Q15 Biquad cascade filter.
-   */
-  typedef struct
-  {
-    int8_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
-    q15_t *pState;            /**< Points to the array of state coefficients.  The array is of length 4*numStages. */
-    q15_t *pCoeffs;           /**< Points to the array of coefficients.  The array is of length 5*numStages. */
-    int8_t postShift;         /**< Additional shift, in bits, applied to each output sample. */
-
-  } arm_biquad_casd_df1_inst_q15;
-
-
-  /**
-   * @brief Instance structure for the Q31 Biquad cascade filter.
-   */
-  typedef struct
-  {
-    uint32_t numStages;      /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
-    q31_t *pState;           /**< Points to the array of state coefficients.  The array is of length 4*numStages. */
-    q31_t *pCoeffs;          /**< Points to the array of coefficients.  The array is of length 5*numStages. */
-    uint8_t postShift;       /**< Additional shift, in bits, applied to each output sample. */
-
-  } arm_biquad_casd_df1_inst_q31;
-
-  /**
-   * @brief Instance structure for the floating-point Biquad cascade filter.
-   */
-  typedef struct
-  {
-    uint32_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
-    float32_t *pState;          /**< Points to the array of state coefficients.  The array is of length 4*numStages. */
-    float32_t *pCoeffs;         /**< Points to the array of coefficients.  The array is of length 5*numStages. */
-
-
-  } arm_biquad_casd_df1_inst_f32;
-
-
-
-  /**
-   * @brief Processing function for the Q15 Biquad cascade filter.
-   * @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.
-   * @return     none.
-   */
-
-  void arm_biquad_cascade_df1_q15(
-  const arm_biquad_casd_df1_inst_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the Q15 Biquad cascade filter.
-   * @param[in,out] *S           points to an instance of the Q15 Biquad cascade 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 to the output. Varies according to the coefficients format
-   * @return        none
-   */
-
-  void arm_biquad_cascade_df1_init_q15(
-  arm_biquad_casd_df1_inst_q15 * S,
-  uint8_t numStages,
-  q15_t * pCoeffs,
-  q15_t * pState,
-  int8_t postShift);
-
-
-  /**
-   * @brief Fast but less precise processing function for the Q15 Biquad cascade filter for Cortex-M3 and Cortex-M4.
-   * @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.
-   * @return     none.
-   */
-
-  void arm_biquad_cascade_df1_fast_q15(
-  const arm_biquad_casd_df1_inst_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Processing function for the Q31 Biquad cascade filter
-   * @param[in]  *S         points to an instance of the Q31 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.
-   * @return     none.
-   */
-
-  void arm_biquad_cascade_df1_q31(
-  const arm_biquad_casd_df1_inst_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Fast but less precise processing function for the Q31 Biquad cascade filter for Cortex-M3 and Cortex-M4.
-   * @param[in]  *S         points to an instance of the Q31 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.
-   * @return     none.
-   */
-
-  void arm_biquad_cascade_df1_fast_q31(
-  const arm_biquad_casd_df1_inst_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the Q31 Biquad cascade filter.
-   * @param[in,out] *S           points to an instance of the Q31 Biquad cascade 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 to the output. Varies according to the coefficients format
-   * @return        none
-   */
-
-  void arm_biquad_cascade_df1_init_q31(
-  arm_biquad_casd_df1_inst_q31 * S,
-  uint8_t numStages,
-  q31_t * pCoeffs,
-  q31_t * pState,
-  int8_t postShift);
-
-  /**
-   * @brief Processing function for the floating-point Biquad cascade filter.
-   * @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.
-   * @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);
-
-  /**
-   * @brief  Initialization function for the floating-point Biquad cascade filter.
-   * @param[in,out] *S           points to an instance of the floating-point Biquad cascade 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.
-   * @return        none
-   */
-
-  void arm_biquad_cascade_df1_init_f32(
-  arm_biquad_casd_df1_inst_f32 * S,
-  uint8_t numStages,
-  float32_t * pCoeffs,
-  float32_t * pState);
-
-
-  /**
-   * @brief Instance structure for the floating-point matrix structure.
-   */
-
-  typedef struct
-  {
-    uint16_t numRows;     /**< number of rows of the matrix.     */
-    uint16_t numCols;     /**< number of columns of the matrix.  */
-    float32_t *pData;     /**< points to the data of the matrix. */
-  } arm_matrix_instance_f32;
-
-  /**
-   * @brief Instance structure for the Q15 matrix structure.
-   */
-
-  typedef struct
-  {
-    uint16_t numRows;     /**< number of rows of the matrix.     */
-    uint16_t numCols;     /**< number of columns of the matrix.  */
-    q15_t *pData;         /**< points to the data of the matrix. */
-
-  } arm_matrix_instance_q15;
-
-  /**
-   * @brief Instance structure for the Q31 matrix structure.
-   */
-
-  typedef struct
-  {
-    uint16_t numRows;     /**< number of rows of the matrix.     */
-    uint16_t numCols;     /**< number of columns of the matrix.  */
-    q31_t *pData;         /**< points to the data of the matrix. */
-
-  } arm_matrix_instance_q31;
-
-
-
-  /**
-   * @brief Floating-point matrix addition.
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_add_f32(
-  const arm_matrix_instance_f32 * pSrcA,
-  const arm_matrix_instance_f32 * pSrcB,
-  arm_matrix_instance_f32 * pDst);
-
-  /**
-   * @brief Q15 matrix addition.
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_add_q15(
-  const arm_matrix_instance_q15 * pSrcA,
-  const arm_matrix_instance_q15 * pSrcB,
-  arm_matrix_instance_q15 * pDst);
-
-  /**
-   * @brief Q31 matrix addition.
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_add_q31(
-  const arm_matrix_instance_q31 * pSrcA,
-  const arm_matrix_instance_q31 * pSrcB,
-  arm_matrix_instance_q31 * pDst);
-
-
-  /**
-   * @brief Floating-point matrix transpose.
-   * @param[in]  *pSrc points to the input matrix
-   * @param[out] *pDst points to the output matrix
-   * @return 	The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
-   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_trans_f32(
-  const arm_matrix_instance_f32 * pSrc,
-  arm_matrix_instance_f32 * pDst);
-
-
-  /**
-   * @brief Q15 matrix transpose.
-   * @param[in]  *pSrc points to the input matrix
-   * @param[out] *pDst points to the output matrix
-   * @return 	The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
-   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_trans_q15(
-  const arm_matrix_instance_q15 * pSrc,
-  arm_matrix_instance_q15 * pDst);
-
-  /**
-   * @brief Q31 matrix transpose.
-   * @param[in]  *pSrc points to the input matrix
-   * @param[out] *pDst points to the output matrix
-   * @return 	The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>
-   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_trans_q31(
-  const arm_matrix_instance_q31 * pSrc,
-  arm_matrix_instance_q31 * pDst);
-
-
-  /**
-   * @brief Floating-point matrix multiplication
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_mult_f32(
-  const arm_matrix_instance_f32 * pSrcA,
-  const arm_matrix_instance_f32 * pSrcB,
-  arm_matrix_instance_f32 * pDst);
-
-  /**
-   * @brief Q15 matrix multiplication
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @param[in]		  *pState points to the array for storing intermediate results
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_mult_q15(
-  const arm_matrix_instance_q15 * pSrcA,
-  const arm_matrix_instance_q15 * pSrcB,
-  arm_matrix_instance_q15 * pDst,
-  q15_t * pState);
-
-  /**
-   * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
-   * @param[in]       *pSrcA  points to the first input matrix structure
-   * @param[in]       *pSrcB  points to the second input matrix structure
-   * @param[out]      *pDst   points to output matrix structure
-   * @param[in]		  *pState points to the array for storing intermediate results
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_mult_fast_q15(
-  const arm_matrix_instance_q15 * pSrcA,
-  const arm_matrix_instance_q15 * pSrcB,
-  arm_matrix_instance_q15 * pDst,
-  q15_t * pState);
-
-  /**
-   * @brief Q31 matrix multiplication
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_mult_q31(
-  const arm_matrix_instance_q31 * pSrcA,
-  const arm_matrix_instance_q31 * pSrcB,
-  arm_matrix_instance_q31 * pDst);
-
-  /**
-   * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_mult_fast_q31(
-  const arm_matrix_instance_q31 * pSrcA,
-  const arm_matrix_instance_q31 * pSrcB,
-  arm_matrix_instance_q31 * pDst);
-
-
-  /**
-   * @brief Floating-point matrix subtraction
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_sub_f32(
-  const arm_matrix_instance_f32 * pSrcA,
-  const arm_matrix_instance_f32 * pSrcB,
-  arm_matrix_instance_f32 * pDst);
-
-  /**
-   * @brief Q15 matrix subtraction
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_sub_q15(
-  const arm_matrix_instance_q15 * pSrcA,
-  const arm_matrix_instance_q15 * pSrcB,
-  arm_matrix_instance_q15 * pDst);
-
-  /**
-   * @brief Q31 matrix subtraction
-   * @param[in]       *pSrcA points to the first input matrix structure
-   * @param[in]       *pSrcB points to the second input matrix structure
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_sub_q31(
-  const arm_matrix_instance_q31 * pSrcA,
-  const arm_matrix_instance_q31 * pSrcB,
-  arm_matrix_instance_q31 * pDst);
-
-  /**
-   * @brief Floating-point matrix scaling.
-   * @param[in]  *pSrc points to the input matrix
-   * @param[in]  scale scale factor
-   * @param[out] *pDst points to the output matrix
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_scale_f32(
-  const arm_matrix_instance_f32 * pSrc,
-  float32_t scale,
-  arm_matrix_instance_f32 * pDst);
-
-  /**
-   * @brief Q15 matrix scaling.
-   * @param[in]       *pSrc points to input matrix
-   * @param[in]       scaleFract fractional portion of the scale factor
-   * @param[in]       shift number of bits to shift the result by
-   * @param[out]      *pDst points to output matrix
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_scale_q15(
-  const arm_matrix_instance_q15 * pSrc,
-  q15_t scaleFract,
-  int32_t shift,
-  arm_matrix_instance_q15 * pDst);
-
-  /**
-   * @brief Q31 matrix scaling.
-   * @param[in]       *pSrc points to input matrix
-   * @param[in]       scaleFract fractional portion of the scale factor
-   * @param[in]       shift number of bits to shift the result by
-   * @param[out]      *pDst points to output matrix structure
-   * @return     The function returns either
-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
-   */
-
-  arm_status arm_mat_scale_q31(
-  const arm_matrix_instance_q31 * pSrc,
-  q31_t scaleFract,
-  int32_t shift,
-  arm_matrix_instance_q31 * pDst);
-
-
-  /**
-   * @brief  Q31 matrix initialization.
-   * @param[in,out] *S             points to an instance of the floating-point matrix structure.
-   * @param[in]     nRows          number of rows in the matrix.
-   * @param[in]     nColumns       number of columns in the matrix.
-   * @param[in]     *pData	       points to the matrix data array.
-   * @return        none
-   */
-
-  void arm_mat_init_q31(
-  arm_matrix_instance_q31 * S,
-  uint16_t nRows,
-  uint16_t nColumns,
-  q31_t * pData);
-
-  /**
-   * @brief  Q15 matrix initialization.
-   * @param[in,out] *S             points to an instance of the floating-point matrix structure.
-   * @param[in]     nRows          number of rows in the matrix.
-   * @param[in]     nColumns       number of columns in the matrix.
-   * @param[in]     *pData	       points to the matrix data array.
-   * @return        none
-   */
-
-  void arm_mat_init_q15(
-  arm_matrix_instance_q15 * S,
-  uint16_t nRows,
-  uint16_t nColumns,
-  q15_t * pData);
-
-  /**
-   * @brief  Floating-point matrix initialization.
-   * @param[in,out] *S             points to an instance of the floating-point matrix structure.
-   * @param[in]     nRows          number of rows in the matrix.
-   * @param[in]     nColumns       number of columns in the matrix.
-   * @param[in]     *pData	       points to the matrix data array.
-   * @return        none
-   */
-
-  void arm_mat_init_f32(
-  arm_matrix_instance_f32 * S,
-  uint16_t nRows,
-  uint16_t nColumns,
-  float32_t * pData);
-
-
-
-  /**
-   * @brief Instance structure for the Q15 PID Control.
-   */
-  typedef struct
-  {
-    q15_t A0;    /**< The derived gain, A0 = Kp + Ki + Kd . */
-#ifdef ARM_MATH_CM0_FAMILY
-    q15_t A1;
-    q15_t A2;
-#else
-    q31_t A1;           /**< The derived gain A1 = -Kp - 2Kd | Kd.*/
-#endif
-    q15_t state[3];       /**< The state array of length 3. */
-    q15_t Kp;           /**< The proportional gain. */
-    q15_t Ki;           /**< The integral gain. */
-    q15_t Kd;           /**< The derivative gain. */
-  } arm_pid_instance_q15;
-
-  /**
-   * @brief Instance structure for the Q31 PID Control.
-   */
-  typedef struct
-  {
-    q31_t A0;            /**< The derived gain, A0 = Kp + Ki + Kd . */
-    q31_t A1;            /**< The derived gain, A1 = -Kp - 2Kd. */
-    q31_t A2;            /**< The derived gain, A2 = Kd . */
-    q31_t state[3];      /**< The state array of length 3. */
-    q31_t Kp;            /**< The proportional gain. */
-    q31_t Ki;            /**< The integral gain. */
-    q31_t Kd;            /**< The derivative gain. */
-
-  } arm_pid_instance_q31;
-
-  /**
-   * @brief Instance structure for the floating-point PID Control.
-   */
-  typedef struct
-  {
-    float32_t A0;          /**< The derived gain, A0 = Kp + Ki + Kd . */
-    float32_t A1;          /**< The derived gain, A1 = -Kp - 2Kd. */
-    float32_t A2;          /**< The derived gain, A2 = Kd . */
-    float32_t state[3];    /**< The state array of length 3. */
-    float32_t Kp;               /**< The proportional gain. */
-    float32_t Ki;               /**< The integral gain. */
-    float32_t Kd;               /**< The derivative gain. */
-  } arm_pid_instance_f32;
-
-
-
-  /**
-   * @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.
-   */
-  void arm_pid_init_f32(
-  arm_pid_instance_f32 * S,
-  int32_t resetStateFlag);
-
-  /**
-   * @brief  Reset function for the floating-point PID Control.
-   * @param[in,out] *S is an instance of the floating-point PID Control structure
-   * @return none
-   */
-  void arm_pid_reset_f32(
-  arm_pid_instance_f32 * S);
-
-
-  /**
-   * @brief  Initialization function for the Q31 PID Control.
-   * @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.
-   */
-  void arm_pid_init_q31(
-  arm_pid_instance_q31 * S,
-  int32_t resetStateFlag);
-
-
-  /**
-   * @brief  Reset function for the Q31 PID Control.
-   * @param[in,out] *S points to an instance of the Q31 PID Control structure
-   * @return none
-   */
-
-  void arm_pid_reset_q31(
-  arm_pid_instance_q31 * S);
-
-  /**
-   * @brief  Initialization function for the Q15 PID Control.
-   * @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.
-   */
-  void arm_pid_init_q15(
-  arm_pid_instance_q15 * S,
-  int32_t resetStateFlag);
-
-  /**
-   * @brief  Reset function for the Q15 PID Control.
-   * @param[in,out] *S points to an instance of the q15 PID Control structure
-   * @return none
-   */
-  void arm_pid_reset_q15(
-  arm_pid_instance_q15 * S);
-
-
-  /**
-   * @brief Instance structure for the floating-point Linear Interpolate function.
-   */
-  typedef struct
-  {
-    uint32_t nValues;           /**< nValues */
-    float32_t x1;               /**< x1 */
-    float32_t xSpacing;         /**< xSpacing */
-    float32_t *pYData;          /**< pointer to the table of Y values */
-  } arm_linear_interp_instance_f32;
-
-  /**
-   * @brief Instance structure for the floating-point bilinear interpolation function.
-   */
-
-  typedef struct
-  {
-    uint16_t numRows;   /**< number of rows in the data table. */
-    uint16_t numCols;   /**< number of columns in the data table. */
-    float32_t *pData;   /**< points to the data table. */
-  } arm_bilinear_interp_instance_f32;
-
-   /**
-   * @brief Instance structure for the Q31 bilinear interpolation function.
-   */
-
-  typedef struct
-  {
-    uint16_t numRows;   /**< number of rows in the data table. */
-    uint16_t numCols;   /**< number of columns in the data table. */
-    q31_t *pData;       /**< points to the data table. */
-  } arm_bilinear_interp_instance_q31;
-
-   /**
-   * @brief Instance structure for the Q15 bilinear interpolation function.
-   */
-
-  typedef struct
-  {
-    uint16_t numRows;   /**< number of rows in the data table. */
-    uint16_t numCols;   /**< number of columns in the data table. */
-    q15_t *pData;       /**< points to the data table. */
-  } arm_bilinear_interp_instance_q15;
-
-   /**
-   * @brief Instance structure for the Q15 bilinear interpolation function.
-   */
-
-  typedef struct
-  {
-    uint16_t numRows;   /**< number of rows in the data table. */
-    uint16_t numCols;   /**< number of columns in the data table. */
-    q7_t *pData;                /**< points to the data table. */
-  } arm_bilinear_interp_instance_q7;
-
-
-  /**
-   * @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.
-   */
-
-  void arm_mult_q7(
-  q7_t * pSrcA,
-  q7_t * pSrcB,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_mult_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_mult_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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);
-
-
-
-
-
-
-  /**
-   * @brief Instance structure for the Q15 CFFT/CIFFT function.
-   */
-
-  typedef struct
-  {
-    uint16_t fftLen;                 /**< length of the FFT. */
-    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
-    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
-    q15_t *pTwiddle;                     /**< points to the Sin twiddle factor table. */
-    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
-    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
-    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
-  } arm_cfft_radix2_instance_q15;
-
-  arm_status arm_cfft_radix2_init_q15(
-  arm_cfft_radix2_instance_q15 * S,
-  uint16_t fftLen,
-  uint8_t ifftFlag,
-  uint8_t bitReverseFlag);
-
-  void arm_cfft_radix2_q15(
-  const arm_cfft_radix2_instance_q15 * S,
-  q15_t * pSrc);
-
-
-
-  /**
-   * @brief Instance structure for the Q15 CFFT/CIFFT function.
-   */
-
-  typedef struct
-  {
-    uint16_t fftLen;                 /**< length of the FFT. */
-    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
-    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
-    q15_t *pTwiddle;                 /**< points to the twiddle factor table. */
-    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
-    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
-    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
-  } arm_cfft_radix4_instance_q15;
-
-  arm_status arm_cfft_radix4_init_q15(
-  arm_cfft_radix4_instance_q15 * S,
-  uint16_t fftLen,
-  uint8_t ifftFlag,
-  uint8_t bitReverseFlag);
-
-  void arm_cfft_radix4_q15(
-  const arm_cfft_radix4_instance_q15 * S,
-  q15_t * pSrc);
-
-  /**
-   * @brief Instance structure for the Radix-2 Q31 CFFT/CIFFT function.
-   */
-
-  typedef struct
-  {
-    uint16_t fftLen;                 /**< length of the FFT. */
-    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
-    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
-    q31_t *pTwiddle;                     /**< points to the Twiddle factor table. */
-    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
-    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
-    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
-  } arm_cfft_radix2_instance_q31;
-
-  arm_status arm_cfft_radix2_init_q31(
-  arm_cfft_radix2_instance_q31 * S,
-  uint16_t fftLen,
-  uint8_t ifftFlag,
-  uint8_t bitReverseFlag);
-
-  void arm_cfft_radix2_q31(
-  const arm_cfft_radix2_instance_q31 * S,
-  q31_t * pSrc);
-
-  /**
-   * @brief Instance structure for the Q31 CFFT/CIFFT function.
-   */
-
-  typedef struct
-  {
-    uint16_t fftLen;                 /**< length of the FFT. */
-    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
-    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
-    q31_t *pTwiddle;                 /**< points to the twiddle factor table. */
-    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */
-    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
-    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
-  } arm_cfft_radix4_instance_q31;
-
-
-  void arm_cfft_radix4_q31(
-  const arm_cfft_radix4_instance_q31 * S,
-  q31_t * pSrc);
-
-  arm_status arm_cfft_radix4_init_q31(
-  arm_cfft_radix4_instance_q31 * S,
-  uint16_t fftLen,
-  uint8_t ifftFlag,
-  uint8_t bitReverseFlag);
-
-  /**
-   * @brief Instance structure for the floating-point CFFT/CIFFT function.
-   */
-
-  typedef struct
-  {
-    uint16_t fftLen;                   /**< length of the FFT. */
-    uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
-    uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
-    float32_t *pTwiddle;               /**< points to the Twiddle factor table. */
-    uint16_t *pBitRevTable;            /**< points to the bit reversal table. */
-    uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
-    uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
-    float32_t onebyfftLen;                 /**< value of 1/fftLen. */
-  } arm_cfft_radix2_instance_f32;
-
-/* Deprecated */
-  arm_status arm_cfft_radix2_init_f32(
-  arm_cfft_radix2_instance_f32 * S,
-  uint16_t fftLen,
-  uint8_t ifftFlag,
-  uint8_t bitReverseFlag);
-
-/* Deprecated */
-  void arm_cfft_radix2_f32(
-  const arm_cfft_radix2_instance_f32 * S,
-  float32_t * pSrc);
-
-  /**
-   * @brief Instance structure for the floating-point CFFT/CIFFT function.
-   */
-
-  typedef struct
-  {
-    uint16_t fftLen;                   /**< length of the FFT. */
-    uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
-    uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
-    float32_t *pTwiddle;               /**< points to the Twiddle factor table. */
-    uint16_t *pBitRevTable;            /**< points to the bit reversal table. */
-    uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
-    uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
-    float32_t onebyfftLen;                 /**< value of 1/fftLen. */
-  } arm_cfft_radix4_instance_f32;
-
-/* Deprecated */
-  arm_status arm_cfft_radix4_init_f32(
-  arm_cfft_radix4_instance_f32 * S,
-  uint16_t fftLen,
-  uint8_t ifftFlag,
-  uint8_t bitReverseFlag);
-
-/* Deprecated */
-  void arm_cfft_radix4_f32(
-  const arm_cfft_radix4_instance_f32 * S,
-  float32_t * pSrc);
-
-  /**
-   * @brief Instance structure for the floating-point CFFT/CIFFT function.
-   */
-
-  typedef struct
-  {
-    uint16_t fftLen;                   /**< length of the FFT. */
-    const float32_t *pTwiddle;         /**< points to the Twiddle factor table. */
-    const uint16_t *pBitRevTable;      /**< points to the bit reversal table. */
-    uint16_t bitRevLength;             /**< bit reversal table length. */
-  } arm_cfft_instance_f32;
-
-  void arm_cfft_f32(
-  const arm_cfft_instance_f32 * S,
-  float32_t * p1,
-  uint8_t ifftFlag,
-  uint8_t bitReverseFlag);
-
-  /**
-   * @brief Instance structure for the Q15 RFFT/RIFFT function.
-   */
-
-  typedef struct
-  {
-    uint32_t fftLenReal;                      /**< length of the real FFT. */
-    uint32_t fftLenBy2;                       /**< length of the complex FFT. */
-    uint8_t ifftFlagR;                        /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
-    uint8_t bitReverseFlagR;                      /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
-    uint32_t twidCoefRModifier;               /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
-    q15_t *pTwiddleAReal;                     /**< points to the real twiddle factor table. */
-    q15_t *pTwiddleBReal;                     /**< points to the imag twiddle factor table. */
-    arm_cfft_radix4_instance_q15 *pCfft;          /**< points to the complex FFT instance. */
-  } arm_rfft_instance_q15;
-
-  arm_status arm_rfft_init_q15(
-  arm_rfft_instance_q15 * S,
-  arm_cfft_radix4_instance_q15 * S_CFFT,
-  uint32_t fftLenReal,
-  uint32_t ifftFlagR,
-  uint32_t bitReverseFlag);
-
-  void arm_rfft_q15(
-  const arm_rfft_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst);
-
-  /**
-   * @brief Instance structure for the Q31 RFFT/RIFFT function.
-   */
-
-  typedef struct
-  {
-    uint32_t fftLenReal;                        /**< length of the real FFT. */
-    uint32_t fftLenBy2;                         /**< length of the complex FFT. */
-    uint8_t ifftFlagR;                          /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
-    uint8_t bitReverseFlagR;                        /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
-    uint32_t twidCoefRModifier;                 /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
-    q31_t *pTwiddleAReal;                       /**< points to the real twiddle factor table. */
-    q31_t *pTwiddleBReal;                       /**< points to the imag twiddle factor table. */
-    arm_cfft_radix4_instance_q31 *pCfft;        /**< points to the complex FFT instance. */
-  } arm_rfft_instance_q31;
-
-  arm_status arm_rfft_init_q31(
-  arm_rfft_instance_q31 * S,
-  arm_cfft_radix4_instance_q31 * S_CFFT,
-  uint32_t fftLenReal,
-  uint32_t ifftFlagR,
-  uint32_t bitReverseFlag);
-
-  void arm_rfft_q31(
-  const arm_rfft_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst);
-
-  /**
-   * @brief Instance structure for the floating-point RFFT/RIFFT function.
-   */
-
-  typedef struct
-  {
-    uint32_t fftLenReal;                        /**< length of the real FFT. */
-    uint16_t fftLenBy2;                         /**< length of the complex FFT. */
-    uint8_t ifftFlagR;                          /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
-    uint8_t bitReverseFlagR;                    /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
-    uint32_t twidCoefRModifier;                     /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
-    float32_t *pTwiddleAReal;                   /**< points to the real twiddle factor table. */
-    float32_t *pTwiddleBReal;                   /**< points to the imag twiddle factor table. */
-    arm_cfft_radix4_instance_f32 *pCfft;        /**< points to the complex FFT instance. */
-  } arm_rfft_instance_f32;
-
-  arm_status arm_rfft_init_f32(
-  arm_rfft_instance_f32 * S,
-  arm_cfft_radix4_instance_f32 * S_CFFT,
-  uint32_t fftLenReal,
-  uint32_t ifftFlagR,
-  uint32_t bitReverseFlag);
-
-  void arm_rfft_f32(
-  const arm_rfft_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pDst);
-
-  /**
-   * @brief Instance structure for the floating-point RFFT/RIFFT function.
-   */
-
-typedef struct
-  {
-    arm_cfft_instance_f32 Sint;      /**< Internal CFFT structure. */
-    uint16_t fftLenRFFT;                        /**< length of the real sequence */
-	float32_t * pTwiddleRFFT;					/**< Twiddle factors real stage  */
-  } arm_rfft_fast_instance_f32 ;
-
-arm_status arm_rfft_fast_init_f32 (
-	arm_rfft_fast_instance_f32 * S,
-	uint16_t fftLen);
-
-void arm_rfft_fast_f32(
-  arm_rfft_fast_instance_f32 * S,
-  float32_t * p, float32_t * pOut,
-  uint8_t ifftFlag);
-
-  /**
-   * @brief Instance structure for the floating-point DCT4/IDCT4 function.
-   */
-
-  typedef struct
-  {
-    uint16_t N;                         /**< length of the DCT4. */
-    uint16_t Nby2;                      /**< half of the length of the DCT4. */
-    float32_t normalize;                /**< normalizing factor. */
-    float32_t *pTwiddle;                /**< points to the twiddle factor table. */
-    float32_t *pCosFactor;              /**< points to the cosFactor table. */
-    arm_rfft_instance_f32 *pRfft;        /**< points to the real FFT instance. */
-    arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */
-  } arm_dct4_instance_f32;
-
-  /**
-   * @brief  Initialization function for the floating-point DCT4/IDCT4.
-   * @param[in,out] *S         points to an instance of floating-point DCT4/IDCT4 structure.
-   * @param[in]     *S_RFFT    points to an instance of floating-point RFFT/RIFFT structure.
-   * @param[in]     *S_CFFT    points to an instance of floating-point CFFT/CIFFT structure.
-   * @param[in]     N          length of the DCT4.
-   * @param[in]     Nby2       half of the length of the DCT4.
-   * @param[in]     normalize  normalizing factor.
-   * @return		arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>fftLenReal</code> is not a supported transform length.
-   */
-
-  arm_status arm_dct4_init_f32(
-  arm_dct4_instance_f32 * S,
-  arm_rfft_instance_f32 * S_RFFT,
-  arm_cfft_radix4_instance_f32 * S_CFFT,
-  uint16_t N,
-  uint16_t Nby2,
-  float32_t normalize);
-
-  /**
-   * @brief Processing function for the floating-point DCT4/IDCT4.
-   * @param[in]       *S             points to an instance of the floating-point DCT4/IDCT4 structure.
-   * @param[in]       *pState        points to state buffer.
-   * @param[in,out]   *pInlineBuffer points to the in-place input and output buffer.
-   * @return none.
-   */
-
-  void arm_dct4_f32(
-  const arm_dct4_instance_f32 * S,
-  float32_t * pState,
-  float32_t * pInlineBuffer);
-
-  /**
-   * @brief Instance structure for the Q31 DCT4/IDCT4 function.
-   */
-
-  typedef struct
-  {
-    uint16_t N;                         /**< length of the DCT4. */
-    uint16_t Nby2;                      /**< half of the length of the DCT4. */
-    q31_t normalize;                    /**< normalizing factor. */
-    q31_t *pTwiddle;                    /**< points to the twiddle factor table. */
-    q31_t *pCosFactor;                  /**< points to the cosFactor table. */
-    arm_rfft_instance_q31 *pRfft;        /**< points to the real FFT instance. */
-    arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */
-  } arm_dct4_instance_q31;
-
-  /**
-   * @brief  Initialization function for the Q31 DCT4/IDCT4.
-   * @param[in,out] *S         points to an instance of Q31 DCT4/IDCT4 structure.
-   * @param[in]     *S_RFFT    points to an instance of Q31 RFFT/RIFFT structure
-   * @param[in]     *S_CFFT    points to an instance of Q31 CFFT/CIFFT structure
-   * @param[in]     N          length of the DCT4.
-   * @param[in]     Nby2       half of the length of the DCT4.
-   * @param[in]     normalize  normalizing factor.
-   * @return		arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
-   */
-
-  arm_status arm_dct4_init_q31(
-  arm_dct4_instance_q31 * S,
-  arm_rfft_instance_q31 * S_RFFT,
-  arm_cfft_radix4_instance_q31 * S_CFFT,
-  uint16_t N,
-  uint16_t Nby2,
-  q31_t normalize);
-
-  /**
-   * @brief Processing function for the Q31 DCT4/IDCT4.
-   * @param[in]       *S             points to an instance of the Q31 DCT4 structure.
-   * @param[in]       *pState        points to state buffer.
-   * @param[in,out]   *pInlineBuffer points to the in-place input and output buffer.
-   * @return none.
-   */
-
-  void arm_dct4_q31(
-  const arm_dct4_instance_q31 * S,
-  q31_t * pState,
-  q31_t * pInlineBuffer);
-
-  /**
-   * @brief Instance structure for the Q15 DCT4/IDCT4 function.
-   */
-
-  typedef struct
-  {
-    uint16_t N;                         /**< length of the DCT4. */
-    uint16_t Nby2;                      /**< half of the length of the DCT4. */
-    q15_t normalize;                    /**< normalizing factor. */
-    q15_t *pTwiddle;                    /**< points to the twiddle factor table. */
-    q15_t *pCosFactor;                  /**< points to the cosFactor table. */
-    arm_rfft_instance_q15 *pRfft;        /**< points to the real FFT instance. */
-    arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */
-  } arm_dct4_instance_q15;
-
-  /**
-   * @brief  Initialization function for the Q15 DCT4/IDCT4.
-   * @param[in,out] *S         points to an instance of Q15 DCT4/IDCT4 structure.
-   * @param[in]     *S_RFFT    points to an instance of Q15 RFFT/RIFFT structure.
-   * @param[in]     *S_CFFT    points to an instance of Q15 CFFT/CIFFT structure.
-   * @param[in]     N          length of the DCT4.
-   * @param[in]     Nby2       half of the length of the DCT4.
-   * @param[in]     normalize  normalizing factor.
-   * @return		arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
-   */
-
-  arm_status arm_dct4_init_q15(
-  arm_dct4_instance_q15 * S,
-  arm_rfft_instance_q15 * S_RFFT,
-  arm_cfft_radix4_instance_q15 * S_CFFT,
-  uint16_t N,
-  uint16_t Nby2,
-  q15_t normalize);
-
-  /**
-   * @brief Processing function for the Q15 DCT4/IDCT4.
-   * @param[in]       *S             points to an instance of the Q15 DCT4 structure.
-   * @param[in]       *pState        points to state buffer.
-   * @param[in,out]   *pInlineBuffer points to the in-place input and output buffer.
-   * @return none.
-   */
-
-  void arm_dct4_q15(
-  const arm_dct4_instance_q15 * S,
-  q15_t * pState,
-  q15_t * pInlineBuffer);
-
-  /**
-   * @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);
-
-  /**
-   * @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.
-   */
-
-  void arm_add_q7(
-  q7_t * pSrcA,
-  q7_t * pSrcB,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_add_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_add_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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);
-
-  /**
-   * @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.
-   */
-
-  void arm_sub_q7(
-  q7_t * pSrcA,
-  q7_t * pSrcB,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_sub_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_sub_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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);
-
-  /**
-   * @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.
-   */
-
-  void arm_scale_q7(
-  q7_t * pSrc,
-  q7_t scaleFract,
-  int8_t shift,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_scale_q15(
-  q15_t * pSrc,
-  q15_t scaleFract,
-  int8_t shift,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_scale_q31(
-  q31_t * pSrc,
-  q31_t scaleFract,
-  int8_t shift,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_abs_q7(
-  q7_t * pSrc,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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);
-
-  /**
-   * @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.
-   */
-
-  void arm_abs_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_abs_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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);
-
-  /**
-   * @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.
-   */
-
-  void arm_dot_prod_q7(
-  q7_t * pSrcA,
-  q7_t * pSrcB,
-  uint32_t blockSize,
-  q31_t * result);
-
-  /**
-   * @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.
-   */
-
-  void arm_dot_prod_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  uint32_t blockSize,
-  q63_t * result);
-
-  /**
-   * @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.
-   */
-
-  void arm_dot_prod_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  uint32_t blockSize,
-  q63_t * result);
-
-  /**
-   * @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.
-   */
-
-  void arm_shift_q7(
-  q7_t * pSrc,
-  int8_t shiftBits,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_shift_q15(
-  q15_t * pSrc,
-  int8_t shiftBits,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_shift_q31(
-  q31_t * pSrc,
-  int8_t shiftBits,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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);
-
-  /**
-   * @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.
-   */
-
-  void arm_offset_q7(
-  q7_t * pSrc,
-  q7_t offset,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_offset_q15(
-  q15_t * pSrc,
-  q15_t offset,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_offset_q31(
-  q31_t * pSrc,
-  q31_t offset,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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);
-
-  /**
-   * @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.
-   */
-
-  void arm_negate_q7(
-  q7_t * pSrc,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_negate_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @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.
-   */
-
-  void arm_negate_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-  /**
-   * @brief  Copies the elements of a floating-point vector.
-   * @param[in]  *pSrc input pointer
-   * @param[out]  *pDst output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_copy_f32(
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Copies the elements of a Q7 vector.
-   * @param[in]  *pSrc input pointer
-   * @param[out]  *pDst output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_copy_q7(
-  q7_t * pSrc,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Copies the elements of a Q15 vector.
-   * @param[in]  *pSrc input pointer
-   * @param[out]  *pDst output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_copy_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Copies the elements of a Q31 vector.
-   * @param[in]  *pSrc input pointer
-   * @param[out]  *pDst output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_copy_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-  /**
-   * @brief  Fills a constant value into a floating-point vector.
-   * @param[in]  value input value to be filled
-   * @param[out]  *pDst output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_fill_f32(
-  float32_t value,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Fills a constant value into a Q7 vector.
-   * @param[in]  value input value to be filled
-   * @param[out]  *pDst output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_fill_q7(
-  q7_t value,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Fills a constant value into a Q15 vector.
-   * @param[in]  value input value to be filled
-   * @param[out]  *pDst output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_fill_q15(
-  q15_t value,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Fills a constant value into a Q31 vector.
-   * @param[in]  value input value to be filled
-   * @param[out]  *pDst output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_fill_q31(
-  q31_t value,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-/**
- * @brief Convolution of floating-point sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the location where the output result is written.  Length srcALen+srcBLen-1.
- * @return none.
- */
-
-  void arm_conv_f32(
-  float32_t * pSrcA,
-  uint32_t srcALen,
-  float32_t * pSrcB,
-  uint32_t srcBLen,
-  float32_t * pDst);
-
-
-  /**
-   * @brief Convolution of Q15 sequences.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
-   * @param[in]  *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
-   * @param[in]  *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
-   * @return none.
-   */
-
-
-  void arm_conv_opt_q15(
-  q15_t * pSrcA,
-  uint32_t srcALen,
-  q15_t * pSrcB,
-  uint32_t srcBLen,
-  q15_t * pDst,
-  q15_t * pScratch1,
-  q15_t * pScratch2);
-
-
-/**
- * @brief Convolution of Q15 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the location where the output result is written.  Length srcALen+srcBLen-1.
- * @return none.
- */
-
-  void arm_conv_q15(
-  q15_t * pSrcA,
-  uint32_t srcALen,
-  q15_t * pSrcB,
-  uint32_t srcBLen,
-  q15_t * pDst);
-
-  /**
-   * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
-   * @return none.
-   */
-
-  void arm_conv_fast_q15(
-			  q15_t * pSrcA,
-			 uint32_t srcALen,
-			  q15_t * pSrcB,
-			 uint32_t srcBLen,
-			 q15_t * pDst);
-
-  /**
-   * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
-   * @param[in]  *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
-   * @param[in]  *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
-   * @return none.
-   */
-
-  void arm_conv_fast_opt_q15(
-  q15_t * pSrcA,
-  uint32_t srcALen,
-  q15_t * pSrcB,
-  uint32_t srcBLen,
-  q15_t * pDst,
-  q15_t * pScratch1,
-  q15_t * pScratch2);
-
-
-
-  /**
-   * @brief Convolution of Q31 sequences.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
-   * @return none.
-   */
-
-  void arm_conv_q31(
-  q31_t * pSrcA,
-  uint32_t srcALen,
-  q31_t * pSrcB,
-  uint32_t srcBLen,
-  q31_t * pDst);
-
-  /**
-   * @brief Convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
-   * @return none.
-   */
-
-  void arm_conv_fast_q31(
-  q31_t * pSrcA,
-  uint32_t srcALen,
-  q31_t * pSrcB,
-  uint32_t srcBLen,
-  q31_t * pDst);
-
-
-    /**
-   * @brief Convolution of Q7 sequences.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
-   * @param[in]  *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
-   * @param[in]  *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
-   * @return none.
-   */
-
-  void arm_conv_opt_q7(
-  q7_t * pSrcA,
-  uint32_t srcALen,
-  q7_t * pSrcB,
-  uint32_t srcBLen,
-  q7_t * pDst,
-  q15_t * pScratch1,
-  q15_t * pScratch2);
-
-
-
-  /**
-   * @brief Convolution of Q7 sequences.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.
-   * @return none.
-   */
-
-  void arm_conv_q7(
-  q7_t * pSrcA,
-  uint32_t srcALen,
-  q7_t * pSrcB,
-  uint32_t srcBLen,
-  q7_t * pDst);
-
-
-  /**
-   * @brief Partial convolution of floating-point sequences.
-   * @param[in]       *pSrcA points to the first input sequence.
-   * @param[in]       srcALen length of the first input sequence.
-   * @param[in]       *pSrcB points to the second input sequence.
-   * @param[in]       srcBLen length of the second input sequence.
-   * @param[out]      *pDst points to the block of output data
-   * @param[in]       firstIndex is the first output sample to start with.
-   * @param[in]       numPoints is the number of output points to be computed.
-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
-   */
-
-  arm_status arm_conv_partial_f32(
-  float32_t * pSrcA,
-  uint32_t srcALen,
-  float32_t * pSrcB,
-  uint32_t srcBLen,
-  float32_t * pDst,
-  uint32_t firstIndex,
-  uint32_t numPoints);
-
-    /**
-   * @brief Partial convolution of Q15 sequences.
-   * @param[in]       *pSrcA points to the first input sequence.
-   * @param[in]       srcALen length of the first input sequence.
-   * @param[in]       *pSrcB points to the second input sequence.
-   * @param[in]       srcBLen length of the second input sequence.
-   * @param[out]      *pDst points to the block of output data
-   * @param[in]       firstIndex is the first output sample to start with.
-   * @param[in]       numPoints is the number of output points to be computed.
-   * @param[in]       * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
-   * @param[in]       * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
-   */
-
-  arm_status arm_conv_partial_opt_q15(
-  q15_t * pSrcA,
-  uint32_t srcALen,
-  q15_t * pSrcB,
-  uint32_t srcBLen,
-  q15_t * pDst,
-  uint32_t firstIndex,
-  uint32_t numPoints,
-  q15_t * pScratch1,
-  q15_t * pScratch2);
-
-
-/**
-   * @brief Partial convolution of Q15 sequences.
-   * @param[in]       *pSrcA points to the first input sequence.
-   * @param[in]       srcALen length of the first input sequence.
-   * @param[in]       *pSrcB points to the second input sequence.
-   * @param[in]       srcBLen length of the second input sequence.
-   * @param[out]      *pDst points to the block of output data
-   * @param[in]       firstIndex is the first output sample to start with.
-   * @param[in]       numPoints is the number of output points to be computed.
-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
-   */
-
-  arm_status arm_conv_partial_q15(
-  q15_t * pSrcA,
-  uint32_t srcALen,
-  q15_t * pSrcB,
-  uint32_t srcBLen,
-  q15_t * pDst,
-  uint32_t firstIndex,
-  uint32_t numPoints);
-
-  /**
-   * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
-   * @param[in]       *pSrcA points to the first input sequence.
-   * @param[in]       srcALen length of the first input sequence.
-   * @param[in]       *pSrcB points to the second input sequence.
-   * @param[in]       srcBLen length of the second input sequence.
-   * @param[out]      *pDst points to the block of output data
-   * @param[in]       firstIndex is the first output sample to start with.
-   * @param[in]       numPoints is the number of output points to be computed.
-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
-   */
-
-  arm_status arm_conv_partial_fast_q15(
-				        q15_t * pSrcA,
-				       uint32_t srcALen,
-				        q15_t * pSrcB,
-				       uint32_t srcBLen,
-				       q15_t * pDst,
-				       uint32_t firstIndex,
-				       uint32_t numPoints);
-
-
-  /**
-   * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
-   * @param[in]       *pSrcA points to the first input sequence.
-   * @param[in]       srcALen length of the first input sequence.
-   * @param[in]       *pSrcB points to the second input sequence.
-   * @param[in]       srcBLen length of the second input sequence.
-   * @param[out]      *pDst points to the block of output data
-   * @param[in]       firstIndex is the first output sample to start with.
-   * @param[in]       numPoints is the number of output points to be computed.
-   * @param[in]       * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
-   * @param[in]       * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
-   */
-
-  arm_status arm_conv_partial_fast_opt_q15(
-  q15_t * pSrcA,
-  uint32_t srcALen,
-  q15_t * pSrcB,
-  uint32_t srcBLen,
-  q15_t * pDst,
-  uint32_t firstIndex,
-  uint32_t numPoints,
-  q15_t * pScratch1,
-  q15_t * pScratch2);
-
-
-  /**
-   * @brief Partial convolution of Q31 sequences.
-   * @param[in]       *pSrcA points to the first input sequence.
-   * @param[in]       srcALen length of the first input sequence.
-   * @param[in]       *pSrcB points to the second input sequence.
-   * @param[in]       srcBLen length of the second input sequence.
-   * @param[out]      *pDst points to the block of output data
-   * @param[in]       firstIndex is the first output sample to start with.
-   * @param[in]       numPoints is the number of output points to be computed.
-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
-   */
-
-  arm_status arm_conv_partial_q31(
-  q31_t * pSrcA,
-  uint32_t srcALen,
-  q31_t * pSrcB,
-  uint32_t srcBLen,
-  q31_t * pDst,
-  uint32_t firstIndex,
-  uint32_t numPoints);
-
-
-  /**
-   * @brief Partial convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
-   * @param[in]       *pSrcA points to the first input sequence.
-   * @param[in]       srcALen length of the first input sequence.
-   * @param[in]       *pSrcB points to the second input sequence.
-   * @param[in]       srcBLen length of the second input sequence.
-   * @param[out]      *pDst points to the block of output data
-   * @param[in]       firstIndex is the first output sample to start with.
-   * @param[in]       numPoints is the number of output points to be computed.
-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
-   */
-
-  arm_status arm_conv_partial_fast_q31(
-  q31_t * pSrcA,
-  uint32_t srcALen,
-  q31_t * pSrcB,
-  uint32_t srcBLen,
-  q31_t * pDst,
-  uint32_t firstIndex,
-  uint32_t numPoints);
-
-
-  /**
-   * @brief Partial convolution of Q7 sequences
-   * @param[in]       *pSrcA points to the first input sequence.
-   * @param[in]       srcALen length of the first input sequence.
-   * @param[in]       *pSrcB points to the second input sequence.
-   * @param[in]       srcBLen length of the second input sequence.
-   * @param[out]      *pDst points to the block of output data
-   * @param[in]       firstIndex is the first output sample to start with.
-   * @param[in]       numPoints is the number of output points to be computed.
-   * @param[in]  *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
-   * @param[in]  *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
-   */
-
-  arm_status arm_conv_partial_opt_q7(
-  q7_t * pSrcA,
-  uint32_t srcALen,
-  q7_t * pSrcB,
-  uint32_t srcBLen,
-  q7_t * pDst,
-  uint32_t firstIndex,
-  uint32_t numPoints,
-  q15_t * pScratch1,
-  q15_t * pScratch2);
-
-
-/**
-   * @brief Partial convolution of Q7 sequences.
-   * @param[in]       *pSrcA points to the first input sequence.
-   * @param[in]       srcALen length of the first input sequence.
-   * @param[in]       *pSrcB points to the second input sequence.
-   * @param[in]       srcBLen length of the second input sequence.
-   * @param[out]      *pDst points to the block of output data
-   * @param[in]       firstIndex is the first output sample to start with.
-   * @param[in]       numPoints is the number of output points to be computed.
-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
-   */
-
-  arm_status arm_conv_partial_q7(
-  q7_t * pSrcA,
-  uint32_t srcALen,
-  q7_t * pSrcB,
-  uint32_t srcBLen,
-  q7_t * pDst,
-  uint32_t firstIndex,
-  uint32_t numPoints);
-
-
-
-  /**
-   * @brief Instance structure for the Q15 FIR decimator.
-   */
-
-  typedef struct
-  {
-    uint8_t M;                      /**< decimation factor. */
-    uint16_t numTaps;               /**< number of coefficients in the filter. */
-    q15_t *pCoeffs;                  /**< points to the coefficient array. The array is of length numTaps.*/
-    q15_t *pState;                   /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-  } arm_fir_decimate_instance_q15;
-
-  /**
-   * @brief Instance structure for the Q31 FIR decimator.
-   */
-
-  typedef struct
-  {
-    uint8_t M;                  /**< decimation factor. */
-    uint16_t numTaps;           /**< number of coefficients in the filter. */
-    q31_t *pCoeffs;              /**< points to the coefficient array. The array is of length numTaps.*/
-    q31_t *pState;               /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-
-  } arm_fir_decimate_instance_q31;
-
-  /**
-   * @brief Instance structure for the floating-point FIR decimator.
-   */
-
-  typedef struct
-  {
-    uint8_t M;                          /**< decimation factor. */
-    uint16_t numTaps;                   /**< number of coefficients in the filter. */
-    float32_t *pCoeffs;                  /**< points to the coefficient array. The array is of length numTaps.*/
-    float32_t *pState;                   /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-
-  } arm_fir_decimate_instance_f32;
-
-
-
-  /**
-   * @brief Processing function for the floating-point FIR decimator.
-   * @param[in] *S points to an instance of the floating-point FIR decimator 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 input samples to process per call.
-   * @return none
-   */
-
-  void arm_fir_decimate_f32(
-  const arm_fir_decimate_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief  Initialization function for the floating-point FIR decimator.
-   * @param[in,out] *S points to an instance of the floating-point FIR decimator structure.
-   * @param[in] numTaps  number of coefficients in the filter.
-   * @param[in] M  decimation factor.
-   * @param[in] *pCoeffs points to the filter coefficients.
-   * @param[in] *pState points to the state buffer.
-   * @param[in] blockSize number of input samples to process per call.
-   * @return    The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
-   * <code>blockSize</code> is not a multiple of <code>M</code>.
-   */
-
-  arm_status arm_fir_decimate_init_f32(
-  arm_fir_decimate_instance_f32 * S,
-  uint16_t numTaps,
-  uint8_t M,
-  float32_t * pCoeffs,
-  float32_t * pState,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the Q15 FIR decimator.
-   * @param[in] *S points to an instance of the Q15 FIR decimator 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 input samples to process per call.
-   * @return none
-   */
-
-  void arm_fir_decimate_q15(
-  const arm_fir_decimate_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
-   * @param[in] *S points to an instance of the Q15 FIR decimator 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 input samples to process per call.
-   * @return none
-   */
-
-  void arm_fir_decimate_fast_q15(
-  const arm_fir_decimate_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-
-
-  /**
-   * @brief  Initialization function for the Q15 FIR decimator.
-   * @param[in,out] *S points to an instance of the Q15 FIR decimator structure.
-   * @param[in] numTaps  number of coefficients in the filter.
-   * @param[in] M  decimation factor.
-   * @param[in] *pCoeffs points to the filter coefficients.
-   * @param[in] *pState points to the state buffer.
-   * @param[in] blockSize number of input samples to process per call.
-   * @return    The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
-   * <code>blockSize</code> is not a multiple of <code>M</code>.
-   */
-
-  arm_status arm_fir_decimate_init_q15(
-  arm_fir_decimate_instance_q15 * S,
-  uint16_t numTaps,
-  uint8_t M,
-  q15_t * pCoeffs,
-  q15_t * pState,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the Q31 FIR decimator.
-   * @param[in] *S points to an instance of the Q31 FIR decimator 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 input samples to process per call.
-   * @return none
-   */
-
-  void arm_fir_decimate_q31(
-  const arm_fir_decimate_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the Q31 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
-   * @param[in] *S points to an instance of the Q31 FIR decimator 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 input samples to process per call.
-   * @return none
-   */
-
-  void arm_fir_decimate_fast_q31(
-  arm_fir_decimate_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief  Initialization function for the Q31 FIR decimator.
-   * @param[in,out] *S points to an instance of the Q31 FIR decimator structure.
-   * @param[in] numTaps  number of coefficients in the filter.
-   * @param[in] M  decimation factor.
-   * @param[in] *pCoeffs points to the filter coefficients.
-   * @param[in] *pState points to the state buffer.
-   * @param[in] blockSize number of input samples to process per call.
-   * @return    The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
-   * <code>blockSize</code> is not a multiple of <code>M</code>.
-   */
-
-  arm_status arm_fir_decimate_init_q31(
-  arm_fir_decimate_instance_q31 * S,
-  uint16_t numTaps,
-  uint8_t M,
-  q31_t * pCoeffs,
-  q31_t * pState,
-  uint32_t blockSize);
-
-
-
-  /**
-   * @brief Instance structure for the Q15 FIR interpolator.
-   */
-
-  typedef struct
-  {
-    uint8_t L;                      /**< upsample factor. */
-    uint16_t phaseLength;           /**< length of each polyphase filter component. */
-    q15_t *pCoeffs;                 /**< points to the coefficient array. The array is of length L*phaseLength. */
-    q15_t *pState;                  /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
-  } arm_fir_interpolate_instance_q15;
-
-  /**
-   * @brief Instance structure for the Q31 FIR interpolator.
-   */
-
-  typedef struct
-  {
-    uint8_t L;                      /**< upsample factor. */
-    uint16_t phaseLength;           /**< length of each polyphase filter component. */
-    q31_t *pCoeffs;                  /**< points to the coefficient array. The array is of length L*phaseLength. */
-    q31_t *pState;                   /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
-  } arm_fir_interpolate_instance_q31;
-
-  /**
-   * @brief Instance structure for the floating-point FIR interpolator.
-   */
-
-  typedef struct
-  {
-    uint8_t L;                     /**< upsample factor. */
-    uint16_t phaseLength;          /**< length of each polyphase filter component. */
-    float32_t *pCoeffs;             /**< points to the coefficient array. The array is of length L*phaseLength. */
-    float32_t *pState;              /**< points to the state variable array. The array is of length phaseLength+numTaps-1. */
-  } arm_fir_interpolate_instance_f32;
-
-
-  /**
-   * @brief Processing function for the Q15 FIR interpolator.
-   * @param[in] *S        points to an instance of the Q15 FIR interpolator 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 input samples to process per call.
-   * @return none.
-   */
-
-  void arm_fir_interpolate_q15(
-  const arm_fir_interpolate_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief  Initialization function for the Q15 FIR interpolator.
-   * @param[in,out] *S        points to an instance of the Q15 FIR interpolator structure.
-   * @param[in]     L         upsample factor.
-   * @param[in]     numTaps   number of filter coefficients in the filter.
-   * @param[in]     *pCoeffs  points to the filter coefficient buffer.
-   * @param[in]     *pState   points to the state buffer.
-   * @param[in]     blockSize number of input samples to process per call.
-   * @return        The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
-   * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
-   */
-
-  arm_status arm_fir_interpolate_init_q15(
-  arm_fir_interpolate_instance_q15 * S,
-  uint8_t L,
-  uint16_t numTaps,
-  q15_t * pCoeffs,
-  q15_t * pState,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the Q31 FIR interpolator.
-   * @param[in] *S        points to an instance of the Q15 FIR interpolator 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 input samples to process per call.
-   * @return none.
-   */
-
-  void arm_fir_interpolate_q31(
-  const arm_fir_interpolate_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the Q31 FIR interpolator.
-   * @param[in,out] *S        points to an instance of the Q31 FIR interpolator structure.
-   * @param[in]     L         upsample factor.
-   * @param[in]     numTaps   number of filter coefficients in the filter.
-   * @param[in]     *pCoeffs  points to the filter coefficient buffer.
-   * @param[in]     *pState   points to the state buffer.
-   * @param[in]     blockSize number of input samples to process per call.
-   * @return        The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
-   * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
-   */
-
-  arm_status arm_fir_interpolate_init_q31(
-  arm_fir_interpolate_instance_q31 * S,
-  uint8_t L,
-  uint16_t numTaps,
-  q31_t * pCoeffs,
-  q31_t * pState,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Processing function for the floating-point FIR interpolator.
-   * @param[in] *S        points to an instance of the floating-point FIR interpolator 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 input samples to process per call.
-   * @return none.
-   */
-
-  void arm_fir_interpolate_f32(
-  const arm_fir_interpolate_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the floating-point FIR interpolator.
-   * @param[in,out] *S        points to an instance of the floating-point FIR interpolator structure.
-   * @param[in]     L         upsample factor.
-   * @param[in]     numTaps   number of filter coefficients in the filter.
-   * @param[in]     *pCoeffs  points to the filter coefficient buffer.
-   * @param[in]     *pState   points to the state buffer.
-   * @param[in]     blockSize number of input samples to process per call.
-   * @return        The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
-   * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
-   */
-
-  arm_status arm_fir_interpolate_init_f32(
-  arm_fir_interpolate_instance_f32 * S,
-  uint8_t L,
-  uint16_t numTaps,
-  float32_t * pCoeffs,
-  float32_t * pState,
-  uint32_t blockSize);
-
-  /**
-   * @brief Instance structure for the high precision Q31 Biquad cascade filter.
-   */
-
-  typedef struct
-  {
-    uint8_t numStages;       /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
-    q63_t *pState;           /**< points to the array of state coefficients.  The array is of length 4*numStages. */
-    q31_t *pCoeffs;          /**< points to the array of coefficients.  The array is of length 5*numStages. */
-    uint8_t postShift;       /**< additional shift, in bits, applied to each output sample. */
-
-  } arm_biquad_cas_df1_32x64_ins_q31;
-
-
-  /**
-   * @param[in]  *S        points to an instance of the high precision Q31 Biquad cascade filter 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.
-   * @return none.
-   */
-
-  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);
-
-
-  /**
-   * @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 to the output. Varies according to the coefficients format
-   * @return        none
-   */
-
-  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);
-
-
-
-  /**
-   * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter.
-   */
-
-  typedef struct
-  {
-    uint8_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */
-    float32_t *pState;         /**< points to the array of state coefficients.  The array is of length 2*numStages. */
-    float32_t *pCoeffs;        /**< points to the array of coefficients.  The array is of length 5*numStages. */
-  } arm_biquad_cascade_df2T_instance_f32;
-
-
-  /**
-   * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
-   * @param[in]  *S        points to an instance of the filter data 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.
-   * @return none.
-   */
-
-  void arm_biquad_cascade_df2T_f32(
-  const arm_biquad_cascade_df2T_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief  Initialization function for the floating-point transposed direct form II Biquad cascade filter.
-   * @param[in,out] *S           points to an instance of the filter data 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.
-   * @return        none
-   */
-
-  void arm_biquad_cascade_df2T_init_f32(
-  arm_biquad_cascade_df2T_instance_f32 * S,
-  uint8_t numStages,
-  float32_t * pCoeffs,
-  float32_t * pState);
-
-
-
-  /**
-   * @brief Instance structure for the Q15 FIR lattice filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numStages;                          /**< number of filter stages. */
-    q15_t *pState;                               /**< points to the state variable array. The array is of length numStages. */
-    q15_t *pCoeffs;                              /**< points to the coefficient array. The array is of length numStages. */
-  } arm_fir_lattice_instance_q15;
-
-  /**
-   * @brief Instance structure for the Q31 FIR lattice filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numStages;                          /**< number of filter stages. */
-    q31_t *pState;                               /**< points to the state variable array. The array is of length numStages. */
-    q31_t *pCoeffs;                              /**< points to the coefficient array. The array is of length numStages. */
-  } arm_fir_lattice_instance_q31;
-
-  /**
-   * @brief Instance structure for the floating-point FIR lattice filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numStages;                  /**< number of filter stages. */
-    float32_t *pState;                   /**< points to the state variable array. The array is of length numStages. */
-    float32_t *pCoeffs;                  /**< points to the coefficient array. The array is of length numStages. */
-  } arm_fir_lattice_instance_f32;
-
-  /**
-   * @brief Initialization function for the Q15 FIR lattice filter.
-   * @param[in] *S points to an instance of the Q15 FIR lattice structure.
-   * @param[in] numStages  number of filter stages.
-   * @param[in] *pCoeffs points to the coefficient buffer.  The array is of length numStages.
-   * @param[in] *pState points to the state buffer.  The array is of length numStages.
-   * @return none.
-   */
-
-  void arm_fir_lattice_init_q15(
-  arm_fir_lattice_instance_q15 * S,
-  uint16_t numStages,
-  q15_t * pCoeffs,
-  q15_t * pState);
-
-
-  /**
-   * @brief Processing function for the Q15 FIR lattice filter.
-   * @param[in] *S points to an instance of the Q15 FIR lattice 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.
-   * @return none.
-   */
-  void arm_fir_lattice_q15(
-  const arm_fir_lattice_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Initialization function for the Q31 FIR lattice filter.
-   * @param[in] *S points to an instance of the Q31 FIR lattice structure.
-   * @param[in] numStages  number of filter stages.
-   * @param[in] *pCoeffs points to the coefficient buffer.  The array is of length numStages.
-   * @param[in] *pState points to the state buffer.   The array is of length numStages.
-   * @return none.
-   */
-
-  void arm_fir_lattice_init_q31(
-  arm_fir_lattice_instance_q31 * S,
-  uint16_t numStages,
-  q31_t * pCoeffs,
-  q31_t * pState);
-
-
-  /**
-   * @brief Processing function for the Q31 FIR lattice filter.
-   * @param[in]  *S        points to an instance of the Q31 FIR lattice 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.
-   * @return none.
-   */
-
-  void arm_fir_lattice_q31(
-  const arm_fir_lattice_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-/**
- * @brief Initialization function for the floating-point FIR lattice filter.
- * @param[in] *S points to an instance of the floating-point FIR lattice structure.
- * @param[in] numStages  number of filter stages.
- * @param[in] *pCoeffs points to the coefficient buffer.  The array is of length numStages.
- * @param[in] *pState points to the state buffer.  The array is of length numStages.
- * @return none.
- */
-
-  void arm_fir_lattice_init_f32(
-  arm_fir_lattice_instance_f32 * S,
-  uint16_t numStages,
-  float32_t * pCoeffs,
-  float32_t * pState);
-
-  /**
-   * @brief Processing function for the floating-point FIR lattice filter.
-   * @param[in]  *S        points to an instance of the floating-point FIR lattice 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.
-   * @return none.
-   */
-
-  void arm_fir_lattice_f32(
-  const arm_fir_lattice_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Instance structure for the Q15 IIR lattice filter.
-   */
-  typedef struct
-  {
-    uint16_t numStages;                         /**< number of stages in the filter. */
-    q15_t *pState;                              /**< points to the state variable array. The array is of length numStages+blockSize. */
-    q15_t *pkCoeffs;                            /**< points to the reflection coefficient array. The array is of length numStages. */
-    q15_t *pvCoeffs;                            /**< points to the ladder coefficient array. The array is of length numStages+1. */
-  } arm_iir_lattice_instance_q15;
-
-  /**
-   * @brief Instance structure for the Q31 IIR lattice filter.
-   */
-  typedef struct
-  {
-    uint16_t numStages;                         /**< number of stages in the filter. */
-    q31_t *pState;                              /**< points to the state variable array. The array is of length numStages+blockSize. */
-    q31_t *pkCoeffs;                            /**< points to the reflection coefficient array. The array is of length numStages. */
-    q31_t *pvCoeffs;                            /**< points to the ladder coefficient array. The array is of length numStages+1. */
-  } arm_iir_lattice_instance_q31;
-
-  /**
-   * @brief Instance structure for the floating-point IIR lattice filter.
-   */
-  typedef struct
-  {
-    uint16_t numStages;                         /**< number of stages in the filter. */
-    float32_t *pState;                          /**< points to the state variable array. The array is of length numStages+blockSize. */
-    float32_t *pkCoeffs;                        /**< points to the reflection coefficient array. The array is of length numStages. */
-    float32_t *pvCoeffs;                        /**< points to the ladder coefficient array. The array is of length numStages+1. */
-  } arm_iir_lattice_instance_f32;
-
-  /**
-   * @brief Processing function for the floating-point IIR lattice filter.
-   * @param[in] *S points to an instance of the floating-point IIR lattice 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.
-   * @return none.
-   */
-
-  void arm_iir_lattice_f32(
-  const arm_iir_lattice_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Initialization function for the floating-point IIR lattice filter.
-   * @param[in] *S points to an instance of the floating-point IIR lattice structure.
-   * @param[in] numStages number of stages in the filter.
-   * @param[in] *pkCoeffs points to the reflection coefficient buffer.  The array is of length numStages.
-   * @param[in] *pvCoeffs points to the ladder coefficient buffer.  The array is of length numStages+1.
-   * @param[in] *pState points to the state buffer.  The array is of length numStages+blockSize-1.
-   * @param[in] blockSize number of samples to process.
-   * @return none.
-   */
-
-  void arm_iir_lattice_init_f32(
-  arm_iir_lattice_instance_f32 * S,
-  uint16_t numStages,
-  float32_t * pkCoeffs,
-  float32_t * pvCoeffs,
-  float32_t * pState,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Processing function for the Q31 IIR lattice filter.
-   * @param[in] *S points to an instance of the Q31 IIR lattice 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.
-   * @return none.
-   */
-
-  void arm_iir_lattice_q31(
-  const arm_iir_lattice_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Initialization function for the Q31 IIR lattice filter.
-   * @param[in] *S points to an instance of the Q31 IIR lattice structure.
-   * @param[in] numStages number of stages in the filter.
-   * @param[in] *pkCoeffs points to the reflection coefficient buffer.  The array is of length numStages.
-   * @param[in] *pvCoeffs points to the ladder coefficient buffer.  The array is of length numStages+1.
-   * @param[in] *pState points to the state buffer.  The array is of length numStages+blockSize.
-   * @param[in] blockSize number of samples to process.
-   * @return none.
-   */
-
-  void arm_iir_lattice_init_q31(
-  arm_iir_lattice_instance_q31 * S,
-  uint16_t numStages,
-  q31_t * pkCoeffs,
-  q31_t * pvCoeffs,
-  q31_t * pState,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Processing function for the Q15 IIR lattice filter.
-   * @param[in] *S points to an instance of the Q15 IIR lattice 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.
-   * @return none.
-   */
-
-  void arm_iir_lattice_q15(
-  const arm_iir_lattice_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-
-/**
- * @brief Initialization function for the Q15 IIR lattice filter.
- * @param[in] *S points to an instance of the fixed-point Q15 IIR lattice structure.
- * @param[in] numStages  number of stages in the filter.
- * @param[in] *pkCoeffs points to reflection coefficient buffer.  The array is of length numStages.
- * @param[in] *pvCoeffs points to ladder coefficient buffer.  The array is of length numStages+1.
- * @param[in] *pState points to state buffer.  The array is of length numStages+blockSize.
- * @param[in] blockSize number of samples to process per call.
- * @return none.
- */
-
-  void arm_iir_lattice_init_q15(
-  arm_iir_lattice_instance_q15 * S,
-  uint16_t numStages,
-  q15_t * pkCoeffs,
-  q15_t * pvCoeffs,
-  q15_t * pState,
-  uint32_t blockSize);
-
-  /**
-   * @brief Instance structure for the floating-point LMS filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numTaps;    /**< number of coefficients in the filter. */
-    float32_t *pState;   /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    float32_t *pCoeffs;  /**< points to the coefficient array. The array is of length numTaps. */
-    float32_t mu;        /**< step size that controls filter coefficient updates. */
-  } arm_lms_instance_f32;
-
-  /**
-   * @brief Processing function for floating-point LMS filter.
-   * @param[in]  *S points to an instance of the floating-point LMS filter structure.
-   * @param[in]  *pSrc points to the block of input data.
-   * @param[in]  *pRef points to the block of reference data.
-   * @param[out] *pOut points to the block of output data.
-   * @param[out] *pErr points to the block of error data.
-   * @param[in]  blockSize number of samples to process.
-   * @return     none.
-   */
-
-  void arm_lms_f32(
-  const arm_lms_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pRef,
-  float32_t * pOut,
-  float32_t * pErr,
-  uint32_t blockSize);
-
-  /**
-   * @brief Initialization function for floating-point LMS filter.
-   * @param[in] *S points to an instance of the floating-point LMS filter structure.
-   * @param[in] numTaps  number of filter coefficients.
-   * @param[in] *pCoeffs points to the coefficient buffer.
-   * @param[in] *pState points to state buffer.
-   * @param[in] mu step size that controls filter coefficient updates.
-   * @param[in] blockSize number of samples to process.
-   * @return none.
-   */
-
-  void arm_lms_init_f32(
-  arm_lms_instance_f32 * S,
-  uint16_t numTaps,
-  float32_t * pCoeffs,
-  float32_t * pState,
-  float32_t mu,
-  uint32_t blockSize);
-
-  /**
-   * @brief Instance structure for the Q15 LMS filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numTaps;    /**< number of coefficients in the filter. */
-    q15_t *pState;       /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    q15_t *pCoeffs;      /**< points to the coefficient array. The array is of length numTaps. */
-    q15_t mu;            /**< step size that controls filter coefficient updates. */
-    uint32_t postShift;  /**< bit shift applied to coefficients. */
-  } arm_lms_instance_q15;
-
-
-  /**
-   * @brief Initialization function for the Q15 LMS filter.
-   * @param[in] *S points to an instance of the Q15 LMS filter structure.
-   * @param[in] numTaps  number of filter coefficients.
-   * @param[in] *pCoeffs points to the coefficient buffer.
-   * @param[in] *pState points to the state buffer.
-   * @param[in] mu step size that controls filter coefficient updates.
-   * @param[in] blockSize number of samples to process.
-   * @param[in] postShift bit shift applied to coefficients.
-   * @return    none.
-   */
-
-  void arm_lms_init_q15(
-  arm_lms_instance_q15 * S,
-  uint16_t numTaps,
-  q15_t * pCoeffs,
-  q15_t * pState,
-  q15_t mu,
-  uint32_t blockSize,
-  uint32_t postShift);
-
-  /**
-   * @brief Processing function for Q15 LMS filter.
-   * @param[in] *S points to an instance of the Q15 LMS filter structure.
-   * @param[in] *pSrc points to the block of input data.
-   * @param[in] *pRef points to the block of reference data.
-   * @param[out] *pOut points to the block of output data.
-   * @param[out] *pErr points to the block of error data.
-   * @param[in] blockSize number of samples to process.
-   * @return none.
-   */
-
-  void arm_lms_q15(
-  const arm_lms_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pRef,
-  q15_t * pOut,
-  q15_t * pErr,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Instance structure for the Q31 LMS filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numTaps;    /**< number of coefficients in the filter. */
-    q31_t *pState;       /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    q31_t *pCoeffs;      /**< points to the coefficient array. The array is of length numTaps. */
-    q31_t mu;            /**< step size that controls filter coefficient updates. */
-    uint32_t postShift;  /**< bit shift applied to coefficients. */
-
-  } arm_lms_instance_q31;
-
-  /**
-   * @brief Processing function for Q31 LMS filter.
-   * @param[in]  *S points to an instance of the Q15 LMS filter structure.
-   * @param[in]  *pSrc points to the block of input data.
-   * @param[in]  *pRef points to the block of reference data.
-   * @param[out] *pOut points to the block of output data.
-   * @param[out] *pErr points to the block of error data.
-   * @param[in]  blockSize number of samples to process.
-   * @return     none.
-   */
-
-  void arm_lms_q31(
-  const arm_lms_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pRef,
-  q31_t * pOut,
-  q31_t * pErr,
-  uint32_t blockSize);
-
-  /**
-   * @brief Initialization function for Q31 LMS filter.
-   * @param[in] *S points to an instance of the Q31 LMS filter structure.
-   * @param[in] numTaps  number of filter coefficients.
-   * @param[in] *pCoeffs points to coefficient buffer.
-   * @param[in] *pState points to state buffer.
-   * @param[in] mu step size that controls filter coefficient updates.
-   * @param[in] blockSize number of samples to process.
-   * @param[in] postShift bit shift applied to coefficients.
-   * @return none.
-   */
-
-  void arm_lms_init_q31(
-  arm_lms_instance_q31 * S,
-  uint16_t numTaps,
-  q31_t * pCoeffs,
-  q31_t * pState,
-  q31_t mu,
-  uint32_t blockSize,
-  uint32_t postShift);
-
-  /**
-   * @brief Instance structure for the floating-point normalized LMS filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numTaps;     /**< number of coefficients in the filter. */
-    float32_t *pState;    /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    float32_t *pCoeffs;   /**< points to the coefficient array. The array is of length numTaps. */
-    float32_t mu;        /**< step size that control filter coefficient updates. */
-    float32_t energy;    /**< saves previous frame energy. */
-    float32_t x0;        /**< saves previous input sample. */
-  } arm_lms_norm_instance_f32;
-
-  /**
-   * @brief Processing function for floating-point normalized LMS filter.
-   * @param[in] *S points to an instance of the floating-point normalized LMS filter structure.
-   * @param[in] *pSrc points to the block of input data.
-   * @param[in] *pRef points to the block of reference data.
-   * @param[out] *pOut points to the block of output data.
-   * @param[out] *pErr points to the block of error data.
-   * @param[in] blockSize number of samples to process.
-   * @return none.
-   */
-
-  void arm_lms_norm_f32(
-  arm_lms_norm_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pRef,
-  float32_t * pOut,
-  float32_t * pErr,
-  uint32_t blockSize);
-
-  /**
-   * @brief Initialization function for floating-point normalized LMS filter.
-   * @param[in] *S points to an instance of the floating-point LMS filter structure.
-   * @param[in] numTaps  number of filter coefficients.
-   * @param[in] *pCoeffs points to coefficient buffer.
-   * @param[in] *pState points to state buffer.
-   * @param[in] mu step size that controls filter coefficient updates.
-   * @param[in] blockSize number of samples to process.
-   * @return none.
-   */
-
-  void arm_lms_norm_init_f32(
-  arm_lms_norm_instance_f32 * S,
-  uint16_t numTaps,
-  float32_t * pCoeffs,
-  float32_t * pState,
-  float32_t mu,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Instance structure for the Q31 normalized LMS filter.
-   */
-  typedef struct
-  {
-    uint16_t numTaps;     /**< number of coefficients in the filter. */
-    q31_t *pState;        /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    q31_t *pCoeffs;       /**< points to the coefficient array. The array is of length numTaps. */
-    q31_t mu;             /**< step size that controls filter coefficient updates. */
-    uint8_t postShift;    /**< bit shift applied to coefficients. */
-    q31_t *recipTable;    /**< points to the reciprocal initial value table. */
-    q31_t energy;         /**< saves previous frame energy. */
-    q31_t x0;             /**< saves previous input sample. */
-  } arm_lms_norm_instance_q31;
-
-  /**
-   * @brief Processing function for Q31 normalized LMS filter.
-   * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
-   * @param[in] *pSrc points to the block of input data.
-   * @param[in] *pRef points to the block of reference data.
-   * @param[out] *pOut points to the block of output data.
-   * @param[out] *pErr points to the block of error data.
-   * @param[in] blockSize number of samples to process.
-   * @return none.
-   */
-
-  void arm_lms_norm_q31(
-  arm_lms_norm_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pRef,
-  q31_t * pOut,
-  q31_t * pErr,
-  uint32_t blockSize);
-
-  /**
-   * @brief Initialization function for Q31 normalized LMS filter.
-   * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
-   * @param[in] numTaps  number of filter coefficients.
-   * @param[in] *pCoeffs points to coefficient buffer.
-   * @param[in] *pState points to state buffer.
-   * @param[in] mu step size that controls filter coefficient updates.
-   * @param[in] blockSize number of samples to process.
-   * @param[in] postShift bit shift applied to coefficients.
-   * @return none.
-   */
-
-  void arm_lms_norm_init_q31(
-  arm_lms_norm_instance_q31 * S,
-  uint16_t numTaps,
-  q31_t * pCoeffs,
-  q31_t * pState,
-  q31_t mu,
-  uint32_t blockSize,
-  uint8_t postShift);
-
-  /**
-   * @brief Instance structure for the Q15 normalized LMS filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numTaps;    /**< Number of coefficients in the filter. */
-    q15_t *pState;        /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-    q15_t *pCoeffs;       /**< points to the coefficient array. The array is of length numTaps. */
-    q15_t mu;            /**< step size that controls filter coefficient updates. */
-    uint8_t postShift;   /**< bit shift applied to coefficients. */
-    q15_t *recipTable;   /**< Points to the reciprocal initial value table. */
-    q15_t energy;        /**< saves previous frame energy. */
-    q15_t x0;            /**< saves previous input sample. */
-  } arm_lms_norm_instance_q15;
-
-  /**
-   * @brief Processing function for Q15 normalized LMS filter.
-   * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
-   * @param[in] *pSrc points to the block of input data.
-   * @param[in] *pRef points to the block of reference data.
-   * @param[out] *pOut points to the block of output data.
-   * @param[out] *pErr points to the block of error data.
-   * @param[in] blockSize number of samples to process.
-   * @return none.
-   */
-
-  void arm_lms_norm_q15(
-  arm_lms_norm_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pRef,
-  q15_t * pOut,
-  q15_t * pErr,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief Initialization function for Q15 normalized LMS filter.
-   * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
-   * @param[in] numTaps  number of filter coefficients.
-   * @param[in] *pCoeffs points to coefficient buffer.
-   * @param[in] *pState points to state buffer.
-   * @param[in] mu step size that controls filter coefficient updates.
-   * @param[in] blockSize number of samples to process.
-   * @param[in] postShift bit shift applied to coefficients.
-   * @return none.
-   */
-
-  void arm_lms_norm_init_q15(
-  arm_lms_norm_instance_q15 * S,
-  uint16_t numTaps,
-  q15_t * pCoeffs,
-  q15_t * pState,
-  q15_t mu,
-  uint32_t blockSize,
-  uint8_t postShift);
-
-  /**
-   * @brief Correlation of floating-point sequences.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
-   * @return none.
-   */
-
-  void arm_correlate_f32(
-  float32_t * pSrcA,
-  uint32_t srcALen,
-  float32_t * pSrcB,
-  uint32_t srcBLen,
-  float32_t * pDst);
-
-
-   /**
-   * @brief Correlation of Q15 sequences
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
-   * @param[in]  *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
-   * @return none.
-   */
-  void arm_correlate_opt_q15(
-  q15_t * pSrcA,
-  uint32_t srcALen,
-  q15_t * pSrcB,
-  uint32_t srcBLen,
-  q15_t * pDst,
-  q15_t * pScratch);
-
-
-  /**
-   * @brief Correlation of Q15 sequences.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
-   * @return none.
-   */
-
-  void arm_correlate_q15(
-  q15_t * pSrcA,
-  uint32_t srcALen,
-  q15_t * pSrcB,
-  uint32_t srcBLen,
-  q15_t * pDst);
-
-  /**
-   * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
-   * @return none.
-   */
-
-  void arm_correlate_fast_q15(
-			       q15_t * pSrcA,
-			      uint32_t srcALen,
-			       q15_t * pSrcB,
-			      uint32_t srcBLen,
-			      q15_t * pDst);
-
-
-
-  /**
-   * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
-   * @param[in]  *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
-   * @return none.
-   */
-
-  void arm_correlate_fast_opt_q15(
-  q15_t * pSrcA,
-  uint32_t srcALen,
-  q15_t * pSrcB,
-  uint32_t srcBLen,
-  q15_t * pDst,
-  q15_t * pScratch);
-
-  /**
-   * @brief Correlation of Q31 sequences.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
-   * @return none.
-   */
-
-  void arm_correlate_q31(
-  q31_t * pSrcA,
-  uint32_t srcALen,
-  q31_t * pSrcB,
-  uint32_t srcBLen,
-  q31_t * pDst);
-
-  /**
-   * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
-   * @return none.
-   */
-
-  void arm_correlate_fast_q31(
-  q31_t * pSrcA,
-  uint32_t srcALen,
-  q31_t * pSrcB,
-  uint32_t srcBLen,
-  q31_t * pDst);
-
-
-
- /**
-   * @brief Correlation of Q7 sequences.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
-   * @param[in]  *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
-   * @param[in]  *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
-   * @return none.
-   */
-
-  void arm_correlate_opt_q7(
-  q7_t * pSrcA,
-  uint32_t srcALen,
-  q7_t * pSrcB,
-  uint32_t srcBLen,
-  q7_t * pDst,
-  q15_t * pScratch1,
-  q15_t * pScratch2);
-
-
-  /**
-   * @brief Correlation of Q7 sequences.
-   * @param[in] *pSrcA points to the first input sequence.
-   * @param[in] srcALen length of the first input sequence.
-   * @param[in] *pSrcB points to the second input sequence.
-   * @param[in] srcBLen length of the second input sequence.
-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.
-   * @return none.
-   */
-
-  void arm_correlate_q7(
-  q7_t * pSrcA,
-  uint32_t srcALen,
-  q7_t * pSrcB,
-  uint32_t srcBLen,
-  q7_t * pDst);
-
-
-  /**
-   * @brief Instance structure for the floating-point sparse FIR filter.
-   */
-  typedef struct
-  {
-    uint16_t numTaps;             /**< number of coefficients in the filter. */
-    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */
-    float32_t *pState;            /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
-    float32_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps.*/
-    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */
-    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */
-  } arm_fir_sparse_instance_f32;
-
-  /**
-   * @brief Instance structure for the Q31 sparse FIR filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numTaps;             /**< number of coefficients in the filter. */
-    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */
-    q31_t *pState;                /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
-    q31_t *pCoeffs;               /**< points to the coefficient array. The array is of length numTaps.*/
-    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */
-    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */
-  } arm_fir_sparse_instance_q31;
-
-  /**
-   * @brief Instance structure for the Q15 sparse FIR filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numTaps;             /**< number of coefficients in the filter. */
-    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */
-    q15_t *pState;                /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
-    q15_t *pCoeffs;               /**< points to the coefficient array. The array is of length numTaps.*/
-    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */
-    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */
-  } arm_fir_sparse_instance_q15;
-
-  /**
-   * @brief Instance structure for the Q7 sparse FIR filter.
-   */
-
-  typedef struct
-  {
-    uint16_t numTaps;             /**< number of coefficients in the filter. */
-    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */
-    q7_t *pState;                 /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
-    q7_t *pCoeffs;                /**< points to the coefficient array. The array is of length numTaps.*/
-    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */
-    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */
-  } arm_fir_sparse_instance_q7;
-
-  /**
-   * @brief Processing function for the floating-point sparse FIR filter.
-   * @param[in]  *S          points to an instance of the floating-point sparse FIR structure.
-   * @param[in]  *pSrc       points to the block of input data.
-   * @param[out] *pDst       points to the block of output data
-   * @param[in]  *pScratchIn points to a temporary buffer of size blockSize.
-   * @param[in]  blockSize   number of input samples to process per call.
-   * @return none.
-   */
-
-  void arm_fir_sparse_f32(
-  arm_fir_sparse_instance_f32 * S,
-  float32_t * pSrc,
-  float32_t * pDst,
-  float32_t * pScratchIn,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the floating-point sparse FIR filter.
-   * @param[in,out] *S         points to an instance of the floating-point sparse FIR structure.
-   * @param[in]     numTaps    number of nonzero coefficients in the filter.
-   * @param[in]     *pCoeffs   points to the array of filter coefficients.
-   * @param[in]     *pState    points to the state buffer.
-   * @param[in]     *pTapDelay points to the array of offset times.
-   * @param[in]     maxDelay   maximum offset time supported.
-   * @param[in]     blockSize  number of samples that will be processed per block.
-   * @return none
-   */
-
-  void arm_fir_sparse_init_f32(
-  arm_fir_sparse_instance_f32 * S,
-  uint16_t numTaps,
-  float32_t * pCoeffs,
-  float32_t * pState,
-  int32_t * pTapDelay,
-  uint16_t maxDelay,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the Q31 sparse FIR filter.
-   * @param[in]  *S          points to an instance of the Q31 sparse FIR structure.
-   * @param[in]  *pSrc       points to the block of input data.
-   * @param[out] *pDst       points to the block of output data
-   * @param[in]  *pScratchIn points to a temporary buffer of size blockSize.
-   * @param[in]  blockSize   number of input samples to process per call.
-   * @return none.
-   */
-
-  void arm_fir_sparse_q31(
-  arm_fir_sparse_instance_q31 * S,
-  q31_t * pSrc,
-  q31_t * pDst,
-  q31_t * pScratchIn,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the Q31 sparse FIR filter.
-   * @param[in,out] *S         points to an instance of the Q31 sparse FIR structure.
-   * @param[in]     numTaps    number of nonzero coefficients in the filter.
-   * @param[in]     *pCoeffs   points to the array of filter coefficients.
-   * @param[in]     *pState    points to the state buffer.
-   * @param[in]     *pTapDelay points to the array of offset times.
-   * @param[in]     maxDelay   maximum offset time supported.
-   * @param[in]     blockSize  number of samples that will be processed per block.
-   * @return none
-   */
-
-  void arm_fir_sparse_init_q31(
-  arm_fir_sparse_instance_q31 * S,
-  uint16_t numTaps,
-  q31_t * pCoeffs,
-  q31_t * pState,
-  int32_t * pTapDelay,
-  uint16_t maxDelay,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the Q15 sparse FIR filter.
-   * @param[in]  *S           points to an instance of the Q15 sparse FIR structure.
-   * @param[in]  *pSrc        points to the block of input data.
-   * @param[out] *pDst        points to the block of output data
-   * @param[in]  *pScratchIn  points to a temporary buffer of size blockSize.
-   * @param[in]  *pScratchOut points to a temporary buffer of size blockSize.
-   * @param[in]  blockSize    number of input samples to process per call.
-   * @return none.
-   */
-
-  void arm_fir_sparse_q15(
-  arm_fir_sparse_instance_q15 * S,
-  q15_t * pSrc,
-  q15_t * pDst,
-  q15_t * pScratchIn,
-  q31_t * pScratchOut,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief  Initialization function for the Q15 sparse FIR filter.
-   * @param[in,out] *S         points to an instance of the Q15 sparse FIR structure.
-   * @param[in]     numTaps    number of nonzero coefficients in the filter.
-   * @param[in]     *pCoeffs   points to the array of filter coefficients.
-   * @param[in]     *pState    points to the state buffer.
-   * @param[in]     *pTapDelay points to the array of offset times.
-   * @param[in]     maxDelay   maximum offset time supported.
-   * @param[in]     blockSize  number of samples that will be processed per block.
-   * @return none
-   */
-
-  void arm_fir_sparse_init_q15(
-  arm_fir_sparse_instance_q15 * S,
-  uint16_t numTaps,
-  q15_t * pCoeffs,
-  q15_t * pState,
-  int32_t * pTapDelay,
-  uint16_t maxDelay,
-  uint32_t blockSize);
-
-  /**
-   * @brief Processing function for the Q7 sparse FIR filter.
-   * @param[in]  *S           points to an instance of the Q7 sparse FIR structure.
-   * @param[in]  *pSrc        points to the block of input data.
-   * @param[out] *pDst        points to the block of output data
-   * @param[in]  *pScratchIn  points to a temporary buffer of size blockSize.
-   * @param[in]  *pScratchOut points to a temporary buffer of size blockSize.
-   * @param[in]  blockSize    number of input samples to process per call.
-   * @return none.
-   */
-
-  void arm_fir_sparse_q7(
-  arm_fir_sparse_instance_q7 * S,
-  q7_t * pSrc,
-  q7_t * pDst,
-  q7_t * pScratchIn,
-  q31_t * pScratchOut,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Initialization function for the Q7 sparse FIR filter.
-   * @param[in,out] *S         points to an instance of the Q7 sparse FIR structure.
-   * @param[in]     numTaps    number of nonzero coefficients in the filter.
-   * @param[in]     *pCoeffs   points to the array of filter coefficients.
-   * @param[in]     *pState    points to the state buffer.
-   * @param[in]     *pTapDelay points to the array of offset times.
-   * @param[in]     maxDelay   maximum offset time supported.
-   * @param[in]     blockSize  number of samples that will be processed per block.
-   * @return none
-   */
-
-  void arm_fir_sparse_init_q7(
-  arm_fir_sparse_instance_q7 * S,
-  uint16_t numTaps,
-  q7_t * pCoeffs,
-  q7_t * pState,
-  int32_t * pTapDelay,
-  uint16_t maxDelay,
-  uint32_t blockSize);
-
-
-  /*
-   * @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 * pCcosVal);
-
-  /*
-   * @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.
-   */
-
-  void arm_sin_cos_q31(
-  q31_t theta,
-  q31_t * pSinVal,
-  q31_t * pCosVal);
-
-
-  /**
-   * @brief  Floating-point complex conjugate.
-   * @param[in]  *pSrc points to the 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_conj_f32(
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t numSamples);
-
-  /**
-   * @brief  Q31 complex conjugate.
-   * @param[in]  *pSrc points to the 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_conj_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t numSamples);
-
-  /**
-   * @brief  Q15 complex conjugate.
-   * @param[in]  *pSrc points to the 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_conj_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t numSamples);
-
-
-
-  /**
-   * @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);
-
-  /**
-   * @brief  Q31 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_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t numSamples);
-
-  /**
-   * @brief  Q15 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_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t numSamples);
-
-
- /**
-   * @ingroup groupController
-   */
-
-  /**
-   * @defgroup PID PID Motor Control
-   *
-   * A Proportional Integral Derivative (PID) controller is a generic feedback control
-   * loop mechanism widely used in industrial control systems.
-   * A PID controller is the most commonly used type of feedback controller.
-   *
-   * This set of functions implements (PID) controllers
-   * for Q15, Q31, and floating-point data types.  The functions operate on a single sample
-   * of data and each call to the function returns a single processed value.
-   * <code>S</code> points to an instance of the PID control data structure.  <code>in</code>
-   * is the input sample value. The functions return the output value.
-   *
-   * \par Algorithm:
-   * <pre>
-   *    y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
-   *    A0 = Kp + Ki + Kd
-   *    A1 = (-Kp ) - (2 * Kd )
-   *    A2 = Kd  </pre>
-   *
-   * \par
-   * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant
-   *
-   * \par
-   * \image html PID.gif "Proportional Integral Derivative Controller"
-   *
-   * \par
-   * The PID controller calculates an "error" value as the difference between
-   * the measured output and the reference input.
-   * The controller attempts to minimize the error by adjusting the process control inputs.
-   * The proportional value determines the reaction to the current error,
-   * the integral value determines the reaction based on the sum of recent errors,
-   * and the derivative value determines the reaction based on the rate at which the error has been changing.
-   *
-   * \par Instance Structure
-   * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure.
-   * A separate instance structure must be defined for each PID Controller.
-   * There are separate instance structure declarations for each of the 3 supported data types.
-   *
-   * \par Reset Functions
-   * There is also an associated reset function for each data type which clears the state array.
-   *
-   * \par Initialization Functions
-   * There is also an associated initialization function for each data type.
-   * The initialization function performs the following operations:
-   * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains.
-   * - Zeros out the values in the state buffer.
-   *
-   * \par
-   * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function.
-   *
-   * \par Fixed-Point Behavior
-   * Care must be taken when using the fixed-point versions of the PID Controller functions.
-   * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
-   * Refer to the function specific documentation below for usage guidelines.
-   */
-
-  /**
-   * @addtogroup PID
-   * @{
-   */
-
-  /**
-   * @brief  Process function for the floating-point PID Control.
-   * @param[in,out] *S is an instance of the floating-point PID Control structure
-   * @param[in] in input sample to process
-   * @return out processed output sample.
-   */
-
-
-  static __INLINE float32_t arm_pid_f32(
-  arm_pid_instance_f32 * S,
-  float32_t in)
-  {
-    float32_t out;
-
-    /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]  */
-    out = (S->A0 * in) +
-      (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);
-
-    /* Update state */
-    S->state[1] = S->state[0];
-    S->state[0] = in;
-    S->state[2] = out;
-
-    /* return to application */
-    return (out);
-
-  }
-
-  /**
-   * @brief  Process function for the Q31 PID Control.
-   * @param[in,out] *S points to an instance of the Q31 PID Control structure
-   * @param[in] in input sample to process
-   * @return out processed output sample.
-   *
-   * <b>Scaling and Overflow Behavior:</b>
-   * \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 as there are four additions.
-   * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
-   */
-
-  static __INLINE q31_t arm_pid_q31(
-  arm_pid_instance_q31 * S,
-  q31_t in)
-  {
-    q63_t acc;
-    q31_t out;
-
-    /* acc = A0 * x[n]  */
-    acc = (q63_t) S->A0 * in;
-
-    /* acc += A1 * x[n-1] */
-    acc += (q63_t) S->A1 * S->state[0];
-
-    /* acc += A2 * x[n-2]  */
-    acc += (q63_t) S->A2 * S->state[1];
-
-    /* convert output to 1.31 format to add y[n-1] */
-    out = (q31_t) (acc >> 31u);
-
-    /* out += y[n-1] */
-    out += S->state[2];
-
-    /* Update state */
-    S->state[1] = S->state[0];
-    S->state[0] = in;
-    S->state[2] = out;
-
-    /* return to application */
-    return (out);
-
-  }
-
-  /**
-   * @brief  Process function for the Q15 PID Control.
-   * @param[in,out] *S points to an instance of the Q15 PID Control structure
-   * @param[in] in input sample to process
-   * @return out processed output sample.
-   *
-   * <b>Scaling and Overflow Behavior:</b>
-   * \par
-   * The function is implemented using a 64-bit internal accumulator.
-   * Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
-   * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
-   * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
-   * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
-   * Lastly, the accumulator is saturated to yield a result in 1.15 format.
-   */
-
-  static __INLINE q15_t arm_pid_q15(
-  arm_pid_instance_q15 * S,
-  q15_t in)
-  {
-    q63_t acc;
-    q15_t out;
-
-#ifndef ARM_MATH_CM0_FAMILY
-    __SIMD32_TYPE *vstate;
-
-    /* Implementation of PID controller */
-
-    /* acc = A0 * x[n]  */
-    acc = (q31_t) __SMUAD(S->A0, in);
-
-    /* acc += A1 * x[n-1] + A2 * x[n-2]  */
-    vstate = __SIMD32_CONST(S->state);
-    acc = __SMLALD(S->A1, (q31_t) *vstate, acc);
-
-#else
-    /* acc = A0 * x[n]  */
-    acc = ((q31_t) S->A0) * in;
-
-    /* acc += A1 * x[n-1] + A2 * x[n-2]  */
-    acc += (q31_t) S->A1 * S->state[0];
-    acc += (q31_t) S->A2 * S->state[1];
-
-#endif
-
-    /* acc += y[n-1] */
-    acc += (q31_t) S->state[2] << 15;
-
-    /* saturate the output */
-    out = (q15_t) (__SSAT((acc >> 15), 16));
-
-    /* Update state */
-    S->state[1] = S->state[0];
-    S->state[0] = in;
-    S->state[2] = out;
-
-    /* return to application */
-    return (out);
-
-  }
-
-  /**
-   * @} end of PID group
-   */
-
-
-  /**
-   * @brief Floating-point matrix inverse.
-   * @param[in]  *src points to the instance of the input floating-point matrix structure.
-   * @param[out] *dst points to the instance of the output floating-point matrix structure.
-   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
-   * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR.
-   */
-
-  arm_status arm_mat_inverse_f32(
-  const arm_matrix_instance_f32 * src,
-  arm_matrix_instance_f32 * dst);
-
-
-
-  /**
-   * @ingroup groupController
-   */
-
-
-  /**
-   * @defgroup clarke Vector Clarke Transform
-   * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector.
-   * Generally the Clarke transform uses three-phase currents <code>Ia, Ib and Ic</code> to calculate currents
-   * in the two-phase orthogonal stator axis <code>Ialpha</code> and <code>Ibeta</code>.
-   * When <code>Ialpha</code> is superposed with <code>Ia</code> as shown in the figure below
-   * \image html clarke.gif Stator current space vector and its components in (a,b).
-   * and <code>Ia + Ib + Ic = 0</code>, in this condition <code>Ialpha</code> and <code>Ibeta</code>
-   * can be calculated using only <code>Ia</code> and <code>Ib</code>.
-   *
-   * The function operates on a single sample of data and each call to the function returns the processed output.
-   * The library provides separate functions for Q31 and floating-point data types.
-   * \par Algorithm
-   * \image html clarkeFormula.gif
-   * where <code>Ia</code> and <code>Ib</code> are the instantaneous stator phases and
-   * <code>pIalpha</code> and <code>pIbeta</code> are the two coordinates of time invariant vector.
-   * \par Fixed-Point Behavior
-   * Care must be taken when using the Q31 version of the Clarke transform.
-   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
-   * Refer to the function specific documentation below for usage guidelines.
-   */
-
-  /**
-   * @addtogroup clarke
-   * @{
-   */
-
-  /**
-   *
-   * @brief  Floating-point Clarke transform
-   * @param[in]       Ia       input three-phase coordinate <code>a</code>
-   * @param[in]       Ib       input three-phase coordinate <code>b</code>
-   * @param[out]      *pIalpha points to output two-phase orthogonal vector axis alpha
-   * @param[out]      *pIbeta  points to output two-phase orthogonal vector axis beta
-   * @return none.
-   */
-
-  static __INLINE void arm_clarke_f32(
-  float32_t Ia,
-  float32_t Ib,
-  float32_t * pIalpha,
-  float32_t * pIbeta)
-  {
-    /* Calculate pIalpha using the equation, pIalpha = Ia */
-    *pIalpha = Ia;
-
-    /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */
-    *pIbeta =
-      ((float32_t) 0.57735026919 * Ia + (float32_t) 1.15470053838 * Ib);
-
-  }
-
-  /**
-   * @brief  Clarke transform for Q31 version
-   * @param[in]       Ia       input three-phase coordinate <code>a</code>
-   * @param[in]       Ib       input three-phase coordinate <code>b</code>
-   * @param[out]      *pIalpha points to output two-phase orthogonal vector axis alpha
-   * @param[out]      *pIbeta  points to output two-phase orthogonal vector axis beta
-   * @return none.
-   *
-   * <b>Scaling and Overflow Behavior:</b>
-   * \par
-   * The function is implemented using an internal 32-bit accumulator.
-   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
-   * There is saturation on the addition, hence there is no risk of overflow.
-   */
-
-  static __INLINE void arm_clarke_q31(
-  q31_t Ia,
-  q31_t Ib,
-  q31_t * pIalpha,
-  q31_t * pIbeta)
-  {
-    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
-
-    /* Calculating pIalpha from Ia by equation pIalpha = Ia */
-    *pIalpha = Ia;
-
-    /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */
-    product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30);
-
-    /* Intermediate product is calculated by (2/sqrt(3) * Ib) */
-    product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30);
-
-    /* pIbeta is calculated by adding the intermediate products */
-    *pIbeta = __QADD(product1, product2);
-  }
-
-  /**
-   * @} end of clarke group
-   */
-
-  /**
-   * @brief  Converts the elements of the Q7 vector to Q31 vector.
-   * @param[in]  *pSrc     input pointer
-   * @param[out]  *pDst    output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_q7_to_q31(
-  q7_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-
-
-
-  /**
-   * @ingroup groupController
-   */
-
-  /**
-   * @defgroup inv_clarke Vector Inverse Clarke Transform
-   * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases.
-   *
-   * The function operates on a single sample of data and each call to the function returns the processed output.
-   * The library provides separate functions for Q31 and floating-point data types.
-   * \par Algorithm
-   * \image html clarkeInvFormula.gif
-   * where <code>pIa</code> and <code>pIb</code> are the instantaneous stator phases and
-   * <code>Ialpha</code> and <code>Ibeta</code> are the two coordinates of time invariant vector.
-   * \par Fixed-Point Behavior
-   * Care must be taken when using the Q31 version of the Clarke transform.
-   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
-   * Refer to the function specific documentation below for usage guidelines.
-   */
-
-  /**
-   * @addtogroup inv_clarke
-   * @{
-   */
-
-   /**
-   * @brief  Floating-point Inverse Clarke transform
-   * @param[in]       Ialpha  input two-phase orthogonal vector axis alpha
-   * @param[in]       Ibeta   input two-phase orthogonal vector axis beta
-   * @param[out]      *pIa    points to output three-phase coordinate <code>a</code>
-   * @param[out]      *pIb    points to output three-phase coordinate <code>b</code>
-   * @return none.
-   */
-
-
-  static __INLINE void arm_inv_clarke_f32(
-  float32_t Ialpha,
-  float32_t Ibeta,
-  float32_t * pIa,
-  float32_t * pIb)
-  {
-    /* Calculating pIa from Ialpha by equation pIa = Ialpha */
-    *pIa = Ialpha;
-
-    /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
-    *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
-
-  }
-
-  /**
-   * @brief  Inverse Clarke transform for Q31 version
-   * @param[in]       Ialpha  input two-phase orthogonal vector axis alpha
-   * @param[in]       Ibeta   input two-phase orthogonal vector axis beta
-   * @param[out]      *pIa    points to output three-phase coordinate <code>a</code>
-   * @param[out]      *pIb    points to output three-phase coordinate <code>b</code>
-   * @return none.
-   *
-   * <b>Scaling and Overflow Behavior:</b>
-   * \par
-   * The function is implemented using an internal 32-bit accumulator.
-   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
-   * There is saturation on the subtraction, hence there is no risk of overflow.
-   */
-
-  static __INLINE void arm_inv_clarke_q31(
-  q31_t Ialpha,
-  q31_t Ibeta,
-  q31_t * pIa,
-  q31_t * pIb)
-  {
-    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
-
-    /* Calculating pIa from Ialpha by equation pIa = Ialpha */
-    *pIa = Ialpha;
-
-    /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */
-    product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31);
-
-    /* Intermediate product is calculated by (1/sqrt(3) * pIb) */
-    product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31);
-
-    /* pIb is calculated by subtracting the products */
-    *pIb = __QSUB(product2, product1);
-
-  }
-
-  /**
-   * @} end of inv_clarke group
-   */
-
-  /**
-   * @brief  Converts the elements of the Q7 vector to Q15 vector.
-   * @param[in]  *pSrc     input pointer
-   * @param[out] *pDst     output pointer
-   * @param[in]  blockSize number of samples to process
-   * @return none.
-   */
-  void arm_q7_to_q15(
-  q7_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-
-
-  /**
-   * @ingroup groupController
-   */
-
-  /**
-   * @defgroup park Vector Park Transform
-   *
-   * Forward Park transform converts the input two-coordinate vector to flux and torque components.
-   * The Park transform can be used to realize the transformation of the <code>Ialpha</code> and the <code>Ibeta</code> currents
-   * from the stationary to the moving reference frame and control the spatial relationship between
-   * the stator vector current and rotor flux vector.
-   * If we consider the d axis aligned with the rotor flux, the diagram below shows the
-   * current vector and the relationship from the two reference frames:
-   * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame"
-   *
-   * The function operates on a single sample of data and each call to the function returns the processed output.
-   * The library provides separate functions for Q31 and floating-point data types.
-   * \par Algorithm
-   * \image html parkFormula.gif
-   * where <code>Ialpha</code> and <code>Ibeta</code> are the stator vector components,
-   * <code>pId</code> and <code>pIq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
-   * cosine and sine values of theta (rotor flux position).
-   * \par Fixed-Point Behavior
-   * Care must be taken when using the Q31 version of the Park transform.
-   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
-   * Refer to the function specific documentation below for usage guidelines.
-   */
-
-  /**
-   * @addtogroup park
-   * @{
-   */
-
-  /**
-   * @brief Floating-point Park transform
-   * @param[in]       Ialpha input two-phase vector coordinate alpha
-   * @param[in]       Ibeta  input two-phase vector coordinate beta
-   * @param[out]      *pId   points to output	rotor reference frame d
-   * @param[out]      *pIq   points to output	rotor reference frame q
-   * @param[in]       sinVal sine value of rotation angle theta
-   * @param[in]       cosVal cosine value of rotation angle theta
-   * @return none.
-   *
-   * The function implements the forward Park transform.
-   *
-   */
-
-  static __INLINE void arm_park_f32(
-  float32_t Ialpha,
-  float32_t Ibeta,
-  float32_t * pId,
-  float32_t * pIq,
-  float32_t sinVal,
-  float32_t cosVal)
-  {
-    /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */
-    *pId = Ialpha * cosVal + Ibeta * sinVal;
-
-    /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */
-    *pIq = -Ialpha * sinVal + Ibeta * cosVal;
-
-  }
-
-  /**
-   * @brief  Park transform for Q31 version
-   * @param[in]       Ialpha input two-phase vector coordinate alpha
-   * @param[in]       Ibeta  input two-phase vector coordinate beta
-   * @param[out]      *pId   points to output rotor reference frame d
-   * @param[out]      *pIq   points to output rotor reference frame q
-   * @param[in]       sinVal sine value of rotation angle theta
-   * @param[in]       cosVal cosine value of rotation angle theta
-   * @return none.
-   *
-   * <b>Scaling and Overflow Behavior:</b>
-   * \par
-   * The function is implemented using an internal 32-bit accumulator.
-   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
-   * There is saturation on the addition and subtraction, hence there is no risk of overflow.
-   */
-
-
-  static __INLINE void arm_park_q31(
-  q31_t Ialpha,
-  q31_t Ibeta,
-  q31_t * pId,
-  q31_t * pIq,
-  q31_t sinVal,
-  q31_t cosVal)
-  {
-    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
-    q31_t product3, product4;                    /* Temporary variables used to store intermediate results */
-
-    /* Intermediate product is calculated by (Ialpha * cosVal) */
-    product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31);
-
-    /* Intermediate product is calculated by (Ibeta * sinVal) */
-    product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31);
-
-
-    /* Intermediate product is calculated by (Ialpha * sinVal) */
-    product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31);
-
-    /* Intermediate product is calculated by (Ibeta * cosVal) */
-    product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31);
-
-    /* Calculate pId by adding the two intermediate products 1 and 2 */
-    *pId = __QADD(product1, product2);
-
-    /* Calculate pIq by subtracting the two intermediate products 3 from 4 */
-    *pIq = __QSUB(product4, product3);
-  }
-
-  /**
-   * @} end of park group
-   */
-
-  /**
-   * @brief  Converts the elements of the Q7 vector to floating-point vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[out]  *pDst is output pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @return none.
-   */
-  void arm_q7_to_float(
-  q7_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @ingroup groupController
-   */
-
-  /**
-   * @defgroup inv_park Vector Inverse Park transform
-   * Inverse Park transform converts the input flux and torque components to two-coordinate vector.
-   *
-   * The function operates on a single sample of data and each call to the function returns the processed output.
-   * The library provides separate functions for Q31 and floating-point data types.
-   * \par Algorithm
-   * \image html parkInvFormula.gif
-   * where <code>pIalpha</code> and <code>pIbeta</code> are the stator vector components,
-   * <code>Id</code> and <code>Iq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
-   * cosine and sine values of theta (rotor flux position).
-   * \par Fixed-Point Behavior
-   * Care must be taken when using the Q31 version of the Park transform.
-   * In particular, the overflow and saturation behavior of the accumulator used must be considered.
-   * Refer to the function specific documentation below for usage guidelines.
-   */
-
-  /**
-   * @addtogroup inv_park
-   * @{
-   */
-
-   /**
-   * @brief  Floating-point Inverse Park transform
-   * @param[in]       Id        input coordinate of rotor reference frame d
-   * @param[in]       Iq        input coordinate of rotor reference frame q
-   * @param[out]      *pIalpha  points to output two-phase orthogonal vector axis alpha
-   * @param[out]      *pIbeta   points to output two-phase orthogonal vector axis beta
-   * @param[in]       sinVal    sine value of rotation angle theta
-   * @param[in]       cosVal    cosine value of rotation angle theta
-   * @return none.
-   */
-
-  static __INLINE void arm_inv_park_f32(
-  float32_t Id,
-  float32_t Iq,
-  float32_t * pIalpha,
-  float32_t * pIbeta,
-  float32_t sinVal,
-  float32_t cosVal)
-  {
-    /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */
-    *pIalpha = Id * cosVal - Iq * sinVal;
-
-    /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */
-    *pIbeta = Id * sinVal + Iq * cosVal;
-
-  }
-
-
-  /**
-   * @brief  Inverse Park transform for	Q31 version
-   * @param[in]       Id        input coordinate of rotor reference frame d
-   * @param[in]       Iq        input coordinate of rotor reference frame q
-   * @param[out]      *pIalpha  points to output two-phase orthogonal vector axis alpha
-   * @param[out]      *pIbeta   points to output two-phase orthogonal vector axis beta
-   * @param[in]       sinVal    sine value of rotation angle theta
-   * @param[in]       cosVal    cosine value of rotation angle theta
-   * @return none.
-   *
-   * <b>Scaling and Overflow Behavior:</b>
-   * \par
-   * The function is implemented using an internal 32-bit accumulator.
-   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
-   * There is saturation on the addition, hence there is no risk of overflow.
-   */
-
-
-  static __INLINE void arm_inv_park_q31(
-  q31_t Id,
-  q31_t Iq,
-  q31_t * pIalpha,
-  q31_t * pIbeta,
-  q31_t sinVal,
-  q31_t cosVal)
-  {
-    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */
-    q31_t product3, product4;                    /* Temporary variables used to store intermediate results */
-
-    /* Intermediate product is calculated by (Id * cosVal) */
-    product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31);
-
-    /* Intermediate product is calculated by (Iq * sinVal) */
-    product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31);
-
-
-    /* Intermediate product is calculated by (Id * sinVal) */
-    product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31);
-
-    /* Intermediate product is calculated by (Iq * cosVal) */
-    product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31);
-
-    /* Calculate pIalpha by using the two intermediate products 1 and 2 */
-    *pIalpha = __QSUB(product1, product2);
-
-    /* Calculate pIbeta by using the two intermediate products 3 and 4 */
-    *pIbeta = __QADD(product4, product3);
-
-  }
-
-  /**
-   * @} end of Inverse park group
-   */
-
-
-  /**
-   * @brief  Converts the elements of the Q31 vector to floating-point vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[out]  *pDst is output pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @return none.
-   */
-  void arm_q31_to_float(
-  q31_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @ingroup groupInterpolation
-   */
-
-  /**
-   * @defgroup LinearInterpolate Linear Interpolation
-   *
-   * Linear interpolation is a method of curve fitting using linear polynomials.
-   * Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line
-   *
-   * \par
-   * \image html LinearInterp.gif "Linear interpolation"
-   *
-   * \par
-   * A  Linear Interpolate function calculates an output value(y), for the input(x)
-   * using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values)
-   *
-   * \par Algorithm:
-   * <pre>
-   *       y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
-   *       where x0, x1 are nearest values of input x
-   *             y0, y1 are nearest values to output y
-   * </pre>
-   *
-   * \par
-   * This set of functions implements Linear interpolation process
-   * for Q7, Q15, Q31, and floating-point data types.  The functions operate on a single
-   * sample of data and each call to the function returns a single processed value.
-   * <code>S</code> points to an instance of the Linear Interpolate function data structure.
-   * <code>x</code> is the input sample value. The functions returns the output value.
-   *
-   * \par
-   * if x is outside of the table boundary, Linear interpolation returns first value of the table
-   * if x is below input range and returns last value of table if x is above range.
-   */
-
-  /**
-   * @addtogroup LinearInterpolate
-   * @{
-   */
-
-  /**
-   * @brief  Process function for the floating-point Linear Interpolation Function.
-   * @param[in,out] *S is an instance of the floating-point Linear Interpolation structure
-   * @param[in] x input sample to process
-   * @return y processed output sample.
-   *
-   */
-
-  static __INLINE float32_t arm_linear_interp_f32(
-  arm_linear_interp_instance_f32 * S,
-  float32_t x)
-  {
-
-    float32_t y;
-    float32_t x0, x1;                            /* Nearest input values */
-    float32_t y0, y1;                            /* Nearest output values */
-    float32_t xSpacing = S->xSpacing;            /* spacing between input values */
-    int32_t i;                                   /* Index variable */
-    float32_t *pYData = S->pYData;               /* pointer to output table */
-
-    /* Calculation of index */
-    i = (int32_t) ((x - S->x1) / xSpacing);
-
-    if(i < 0)
-    {
-      /* Iniatilize output for below specified range as least output value of table */
-      y = pYData[0];
-    }
-    else if((uint32_t)i >= S->nValues)
-    {
-      /* Iniatilize output for above specified range as last output value of table */
-      y = pYData[S->nValues - 1];
-    }
-    else
-    {
-      /* Calculation of nearest input values */
-      x0 = S->x1 + i * xSpacing;
-      x1 = S->x1 + (i + 1) * xSpacing;
-
-      /* Read of nearest output values */
-      y0 = pYData[i];
-      y1 = pYData[i + 1];
-
-      /* Calculation of output */
-      y = y0 + (x - x0) * ((y1 - y0) / (x1 - x0));
-
-    }
-
-    /* returns output value */
-    return (y);
-  }
-
-   /**
-   *
-   * @brief  Process function for the Q31 Linear Interpolation Function.
-   * @param[in] *pYData  pointer to Q31 Linear Interpolation table
-   * @param[in] x input sample to process
-   * @param[in] nValues number of table values
-   * @return y processed output sample.
-   *
-   * \par
-   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
-   * This function can support maximum of table size 2^12.
-   *
-   */
-
-
-  static __INLINE q31_t arm_linear_interp_q31(
-  q31_t * pYData,
-  q31_t x,
-  uint32_t nValues)
-  {
-    q31_t y;                                     /* output */
-    q31_t y0, y1;                                /* Nearest output values */
-    q31_t fract;                                 /* fractional part */
-    int32_t index;                               /* Index to read nearest output values */
-
-    /* Input is in 12.20 format */
-    /* 12 bits for the table index */
-    /* Index value calculation */
-    index = ((x & 0xFFF00000) >> 20);
-
-    if(index >= (int32_t)(nValues - 1))
-    {
-      return (pYData[nValues - 1]);
-    }
-    else if(index < 0)
-    {
-      return (pYData[0]);
-    }
-    else
-    {
-
-      /* 20 bits for the fractional part */
-      /* shift left by 11 to keep fract in 1.31 format */
-      fract = (x & 0x000FFFFF) << 11;
-
-      /* Read two nearest output values from the index in 1.31(q31) format */
-      y0 = pYData[index];
-      y1 = pYData[index + 1u];
-
-      /* Calculation of y0 * (1-fract) and y is in 2.30 format */
-      y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32));
-
-      /* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */
-      y += ((q31_t) (((q63_t) y1 * fract) >> 32));
-
-      /* Convert y to 1.31 format */
-      return (y << 1u);
-
-    }
-
-  }
-
-  /**
-   *
-   * @brief  Process function for the Q15 Linear Interpolation Function.
-   * @param[in] *pYData  pointer to Q15 Linear Interpolation table
-   * @param[in] x input sample to process
-   * @param[in] nValues number of table values
-   * @return y processed output sample.
-   *
-   * \par
-   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
-   * This function can support maximum of table size 2^12.
-   *
-   */
-
-
-  static __INLINE q15_t arm_linear_interp_q15(
-  q15_t * pYData,
-  q31_t x,
-  uint32_t nValues)
-  {
-    q63_t y;                                     /* output */
-    q15_t y0, y1;                                /* Nearest output values */
-    q31_t fract;                                 /* fractional part */
-    int32_t index;                               /* Index to read nearest output values */
-
-    /* Input is in 12.20 format */
-    /* 12 bits for the table index */
-    /* Index value calculation */
-    index = ((x & 0xFFF00000) >> 20u);
-
-    if(index >= (int32_t)(nValues - 1))
-    {
-      return (pYData[nValues - 1]);
-    }
-    else if(index < 0)
-    {
-      return (pYData[0]);
-    }
-    else
-    {
-      /* 20 bits for the fractional part */
-      /* fract is in 12.20 format */
-      fract = (x & 0x000FFFFF);
-
-      /* Read two nearest output values from the index */
-      y0 = pYData[index];
-      y1 = pYData[index + 1u];
-
-      /* Calculation of y0 * (1-fract) and y is in 13.35 format */
-      y = ((q63_t) y0 * (0xFFFFF - fract));
-
-      /* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */
-      y += ((q63_t) y1 * (fract));
-
-      /* convert y to 1.15 format */
-      return (y >> 20);
-    }
-
-
-  }
-
-  /**
-   *
-   * @brief  Process function for the Q7 Linear Interpolation Function.
-   * @param[in] *pYData  pointer to Q7 Linear Interpolation table
-   * @param[in] x input sample to process
-   * @param[in] nValues number of table values
-   * @return y processed output sample.
-   *
-   * \par
-   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
-   * This function can support maximum of table size 2^12.
-   */
-
-
-  static __INLINE q7_t arm_linear_interp_q7(
-  q7_t * pYData,
-  q31_t x,
-  uint32_t nValues)
-  {
-    q31_t y;                                     /* output */
-    q7_t y0, y1;                                 /* Nearest output values */
-    q31_t fract;                                 /* fractional part */
-    uint32_t index;                              /* Index to read nearest output values */
-
-    /* Input is in 12.20 format */
-    /* 12 bits for the table index */
-    /* Index value calculation */
-    if (x < 0)
-    {
-      return (pYData[0]);
-    }
-    index = (x >> 20) & 0xfff;
-
-
-    if(index >= (nValues - 1))
-    {
-      return (pYData[nValues - 1]);
-    }
-    else
-    {
-
-      /* 20 bits for the fractional part */
-      /* fract is in 12.20 format */
-      fract = (x & 0x000FFFFF);
-
-      /* Read two nearest output values from the index and are in 1.7(q7) format */
-      y0 = pYData[index];
-      y1 = pYData[index + 1u];
-
-      /* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */
-      y = ((y0 * (0xFFFFF - fract)));
-
-      /* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */
-      y += (y1 * fract);
-
-      /* convert y to 1.7(q7) format */
-      return (y >> 20u);
-
-    }
-
-  }
-  /**
-   * @} end of LinearInterpolate group
-   */
-
-  /**
-   * @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);
-
-  /**
-   * @brief  Fast approximation to the trigonometric sine function for Q31 data.
-   * @param[in] x Scaled input value in radians.
-   * @return  sin(x).
-   */
-
-  q31_t arm_sin_q31(
-  q31_t x);
-
-  /**
-   * @brief  Fast approximation to the trigonometric sine function for Q15 data.
-   * @param[in] x Scaled input value in radians.
-   * @return  sin(x).
-   */
-
-  q15_t arm_sin_q15(
-  q15_t x);
-
-  /**
-   * @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);
-
-  /**
-   * @brief Fast approximation to the trigonometric cosine function for Q31 data.
-   * @param[in] x Scaled input value in radians.
-   * @return  cos(x).
-   */
-
-  q31_t arm_cos_q31(
-  q31_t x);
-
-  /**
-   * @brief  Fast approximation to the trigonometric cosine function for Q15 data.
-   * @param[in] x Scaled input value in radians.
-   * @return  cos(x).
-   */
-
-  q15_t arm_cos_q15(
-  q15_t x);
-
-
-  /**
-   * @ingroup groupFastMath
-   */
-
-
-  /**
-   * @defgroup SQRT Square Root
-   *
-   * Computes the square root of a number.
-   * There are separate functions for Q15, Q31, and floating-point data types.
-   * The square root function is computed using the Newton-Raphson algorithm.
-   * This is an iterative algorithm of the form:
-   * <pre>
-   *      x1 = x0 - f(x0)/f'(x0)
-   * </pre>
-   * where <code>x1</code> is the current estimate,
-   * <code>x0</code> is the previous estimate, and
-   * <code>f'(x0)</code> is the derivative of <code>f()</code> evaluated at <code>x0</code>.
-   * For the square root function, the algorithm reduces to:
-   * <pre>
-   *     x0 = in/2                         [initial guess]
-   *     x1 = 1/2 * ( x0 + in / x0)        [each iteration]
-   * </pre>
-   */
-
-
-  /**
-   * @addtogroup SQRT
-   * @{
-   */
-
-  /**
-   * @brief  Floating-point square root function.
-   * @param[in]  in     input value.
-   * @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.
-   */
-
-  static __INLINE arm_status arm_sqrt_f32(
-  float32_t in,
-  float32_t * pOut)
-  {
-    if(in > 0)
-    {
-
-//      #if __FPU_USED
-#if (__FPU_USED == 1) && defined ( __CC_ARM   )
-      *pOut = __sqrtf(in);
-#else
-      *pOut = sqrtf(in);
-#endif
-
-      return (ARM_MATH_SUCCESS);
-    }
-    else
-    {
-      *pOut = 0.0f;
-      return (ARM_MATH_ARGUMENT_ERROR);
-    }
-
-  }
-
-
-  /**
-   * @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);
-
-  /**
-   * @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);
-
-  /**
-   * @} end of SQRT group
-   */
-
-
-
-
-
-
-  /**
-   * @brief floating-point Circular write function.
-   */
-
-  static __INLINE void arm_circularWrite_f32(
-  int32_t * circBuffer,
-  int32_t L,
-  uint16_t * writeOffset,
-  int32_t bufferInc,
-  const int32_t * src,
-  int32_t srcInc,
-  uint32_t blockSize)
-  {
-    uint32_t i = 0u;
-    int32_t wOffset;
-
-    /* Copy the value of Index pointer that points
-     * to the current location where the input samples to be copied */
-    wOffset = *writeOffset;
-
-    /* Loop over the blockSize */
-    i = blockSize;
-
-    while(i > 0u)
-    {
-      /* copy the input sample to the circular buffer */
-      circBuffer[wOffset] = *src;
-
-      /* Update the input pointer */
-      src += srcInc;
-
-      /* Circularly update wOffset.  Watch out for positive and negative value */
-      wOffset += bufferInc;
-      if(wOffset >= L)
-        wOffset -= L;
-
-      /* Decrement the loop counter */
-      i--;
-    }
-
-    /* Update the index pointer */
-    *writeOffset = wOffset;
-  }
-
-
-
-  /**
-   * @brief floating-point Circular Read function.
-   */
-  static __INLINE void arm_circularRead_f32(
-  int32_t * circBuffer,
-  int32_t L,
-  int32_t * readOffset,
-  int32_t bufferInc,
-  int32_t * dst,
-  int32_t * dst_base,
-  int32_t dst_length,
-  int32_t dstInc,
-  uint32_t blockSize)
-  {
-    uint32_t i = 0u;
-    int32_t rOffset, dst_end;
-
-    /* Copy the value of Index pointer that points
-     * to the current location from where the input samples to be read */
-    rOffset = *readOffset;
-    dst_end = (int32_t) (dst_base + dst_length);
-
-    /* Loop over the blockSize */
-    i = blockSize;
-
-    while(i > 0u)
-    {
-      /* copy the sample from the circular buffer to the destination buffer */
-      *dst = circBuffer[rOffset];
-
-      /* Update the input pointer */
-      dst += dstInc;
-
-      if(dst == (int32_t *) dst_end)
-      {
-        dst = dst_base;
-      }
-
-      /* Circularly update rOffset.  Watch out for positive and negative value  */
-      rOffset += bufferInc;
-
-      if(rOffset >= L)
-      {
-        rOffset -= L;
-      }
-
-      /* Decrement the loop counter */
-      i--;
-    }
-
-    /* Update the index pointer */
-    *readOffset = rOffset;
-  }
-
-  /**
-   * @brief Q15 Circular write function.
-   */
-
-  static __INLINE void arm_circularWrite_q15(
-  q15_t * circBuffer,
-  int32_t L,
-  uint16_t * writeOffset,
-  int32_t bufferInc,
-  const q15_t * src,
-  int32_t srcInc,
-  uint32_t blockSize)
-  {
-    uint32_t i = 0u;
-    int32_t wOffset;
-
-    /* Copy the value of Index pointer that points
-     * to the current location where the input samples to be copied */
-    wOffset = *writeOffset;
-
-    /* Loop over the blockSize */
-    i = blockSize;
-
-    while(i > 0u)
-    {
-      /* copy the input sample to the circular buffer */
-      circBuffer[wOffset] = *src;
-
-      /* Update the input pointer */
-      src += srcInc;
-
-      /* Circularly update wOffset.  Watch out for positive and negative value */
-      wOffset += bufferInc;
-      if(wOffset >= L)
-        wOffset -= L;
-
-      /* Decrement the loop counter */
-      i--;
-    }
-
-    /* Update the index pointer */
-    *writeOffset = wOffset;
-  }
-
-
-
-  /**
-   * @brief Q15 Circular Read function.
-   */
-  static __INLINE void arm_circularRead_q15(
-  q15_t * circBuffer,
-  int32_t L,
-  int32_t * readOffset,
-  int32_t bufferInc,
-  q15_t * dst,
-  q15_t * dst_base,
-  int32_t dst_length,
-  int32_t dstInc,
-  uint32_t blockSize)
-  {
-    uint32_t i = 0;
-    int32_t rOffset, dst_end;
-
-    /* Copy the value of Index pointer that points
-     * to the current location from where the input samples to be read */
-    rOffset = *readOffset;
-
-    dst_end = (int32_t) (dst_base + dst_length);
-
-    /* Loop over the blockSize */
-    i = blockSize;
-
-    while(i > 0u)
-    {
-      /* copy the sample from the circular buffer to the destination buffer */
-      *dst = circBuffer[rOffset];
-
-      /* Update the input pointer */
-      dst += dstInc;
-
-      if(dst == (q15_t *) dst_end)
-      {
-        dst = dst_base;
-      }
-
-      /* Circularly update wOffset.  Watch out for positive and negative value */
-      rOffset += bufferInc;
-
-      if(rOffset >= L)
-      {
-        rOffset -= L;
-      }
-
-      /* Decrement the loop counter */
-      i--;
-    }
-
-    /* Update the index pointer */
-    *readOffset = rOffset;
-  }
-
-
-  /**
-   * @brief Q7 Circular write function.
-   */
-
-  static __INLINE void arm_circularWrite_q7(
-  q7_t * circBuffer,
-  int32_t L,
-  uint16_t * writeOffset,
-  int32_t bufferInc,
-  const q7_t * src,
-  int32_t srcInc,
-  uint32_t blockSize)
-  {
-    uint32_t i = 0u;
-    int32_t wOffset;
-
-    /* Copy the value of Index pointer that points
-     * to the current location where the input samples to be copied */
-    wOffset = *writeOffset;
-
-    /* Loop over the blockSize */
-    i = blockSize;
-
-    while(i > 0u)
-    {
-      /* copy the input sample to the circular buffer */
-      circBuffer[wOffset] = *src;
-
-      /* Update the input pointer */
-      src += srcInc;
-
-      /* Circularly update wOffset.  Watch out for positive and negative value */
-      wOffset += bufferInc;
-      if(wOffset >= L)
-        wOffset -= L;
-
-      /* Decrement the loop counter */
-      i--;
-    }
-
-    /* Update the index pointer */
-    *writeOffset = wOffset;
-  }
-
-
-
-  /**
-   * @brief Q7 Circular Read function.
-   */
-  static __INLINE void arm_circularRead_q7(
-  q7_t * circBuffer,
-  int32_t L,
-  int32_t * readOffset,
-  int32_t bufferInc,
-  q7_t * dst,
-  q7_t * dst_base,
-  int32_t dst_length,
-  int32_t dstInc,
-  uint32_t blockSize)
-  {
-    uint32_t i = 0;
-    int32_t rOffset, dst_end;
-
-    /* Copy the value of Index pointer that points
-     * to the current location from where the input samples to be read */
-    rOffset = *readOffset;
-
-    dst_end = (int32_t) (dst_base + dst_length);
-
-    /* Loop over the blockSize */
-    i = blockSize;
-
-    while(i > 0u)
-    {
-      /* copy the sample from the circular buffer to the destination buffer */
-      *dst = circBuffer[rOffset];
-
-      /* Update the input pointer */
-      dst += dstInc;
-
-      if(dst == (q7_t *) dst_end)
-      {
-        dst = dst_base;
-      }
-
-      /* Circularly update rOffset.  Watch out for positive and negative value */
-      rOffset += bufferInc;
-
-      if(rOffset >= L)
-      {
-        rOffset -= L;
-      }
-
-      /* Decrement the loop counter */
-      i--;
-    }
-
-    /* Update the index pointer */
-    *readOffset = rOffset;
-  }
-
-
-  /**
-   * @brief  Sum of the squares of the elements of a Q31 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_power_q31(
-  q31_t * pSrc,
-  uint32_t blockSize,
-  q63_t * pResult);
-
-  /**
-   * @brief  Sum of the squares of the elements of a floating-point vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_power_f32(
-  float32_t * pSrc,
-  uint32_t blockSize,
-  float32_t * pResult);
-
-  /**
-   * @brief  Sum of the squares of the elements of a Q15 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_power_q15(
-  q15_t * pSrc,
-  uint32_t blockSize,
-  q63_t * pResult);
-
-  /**
-   * @brief  Sum of the squares of the elements of a Q7 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_power_q7(
-  q7_t * pSrc,
-  uint32_t blockSize,
-  q31_t * pResult);
-
-  /**
-   * @brief  Mean value of a Q7 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_mean_q7(
-  q7_t * pSrc,
-  uint32_t blockSize,
-  q7_t * pResult);
-
-  /**
-   * @brief  Mean value of a Q15 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-  void arm_mean_q15(
-  q15_t * pSrc,
-  uint32_t blockSize,
-  q15_t * pResult);
-
-  /**
-   * @brief  Mean value of a Q31 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-  void arm_mean_q31(
-  q31_t * pSrc,
-  uint32_t blockSize,
-  q31_t * pResult);
-
-  /**
-   * @brief  Mean value of a floating-point vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-  void arm_mean_f32(
-  float32_t * pSrc,
-  uint32_t blockSize,
-  float32_t * pResult);
-
-  /**
-   * @brief  Variance of the elements of a floating-point vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_var_f32(
-  float32_t * pSrc,
-  uint32_t blockSize,
-  float32_t * pResult);
-
-  /**
-   * @brief  Variance of the elements of a Q31 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_var_q31(
-  q31_t * pSrc,
-  uint32_t blockSize,
-  q63_t * pResult);
-
-  /**
-   * @brief  Variance of the elements of a Q15 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_var_q15(
-  q15_t * pSrc,
-  uint32_t blockSize,
-  q31_t * pResult);
-
-  /**
-   * @brief  Root Mean Square of the elements of a floating-point vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_rms_f32(
-  float32_t * pSrc,
-  uint32_t blockSize,
-  float32_t * pResult);
-
-  /**
-   * @brief  Root Mean Square of the elements of a Q31 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_rms_q31(
-  q31_t * pSrc,
-  uint32_t blockSize,
-  q31_t * pResult);
-
-  /**
-   * @brief  Root Mean Square of the elements of a Q15 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_rms_q15(
-  q15_t * pSrc,
-  uint32_t blockSize,
-  q15_t * pResult);
-
-  /**
-   * @brief  Standard deviation of the elements of a floating-point vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_std_f32(
-  float32_t * pSrc,
-  uint32_t blockSize,
-  float32_t * pResult);
-
-  /**
-   * @brief  Standard deviation of the elements of a Q31 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_std_q31(
-  q31_t * pSrc,
-  uint32_t blockSize,
-  q31_t * pResult);
-
-  /**
-   * @brief  Standard deviation of the elements of a Q15 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output value.
-   * @return none.
-   */
-
-  void arm_std_q15(
-  q15_t * pSrc,
-  uint32_t blockSize,
-  q15_t * pResult);
-
-  /**
-   * @brief  Floating-point complex magnitude
-   * @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_f32(
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t numSamples);
-
-  /**
-   * @brief  Q31 complex magnitude
-   * @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_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t numSamples);
-
-  /**
-   * @brief  Q15 complex magnitude
-   * @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_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t numSamples);
-
-  /**
-   * @brief  Q15 complex dot product
-   * @param[in]  *pSrcA points to the first input vector
-   * @param[in]  *pSrcB points to the second input vector
-   * @param[in]  numSamples number of complex samples in each vector
-   * @param[out]  *realResult real part of the result returned here
-   * @param[out]  *imagResult imaginary part of the result returned here
-   * @return none.
-   */
-
-  void arm_cmplx_dot_prod_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  uint32_t numSamples,
-  q31_t * realResult,
-  q31_t * imagResult);
-
-  /**
-   * @brief  Q31 complex dot product
-   * @param[in]  *pSrcA points to the first input vector
-   * @param[in]  *pSrcB points to the second input vector
-   * @param[in]  numSamples number of complex samples in each vector
-   * @param[out]  *realResult real part of the result returned here
-   * @param[out]  *imagResult imaginary part of the result returned here
-   * @return none.
-   */
-
-  void arm_cmplx_dot_prod_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  uint32_t numSamples,
-  q63_t * realResult,
-  q63_t * imagResult);
-
-  /**
-   * @brief  Floating-point complex dot product
-   * @param[in]  *pSrcA points to the first input vector
-   * @param[in]  *pSrcB points to the second input vector
-   * @param[in]  numSamples number of complex samples in each vector
-   * @param[out]  *realResult real part of the result returned here
-   * @param[out]  *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);
-
-  /**
-   * @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.
-   */
-
-  void arm_cmplx_mult_real_q15(
-  q15_t * pSrcCmplx,
-  q15_t * pSrcReal,
-  q15_t * pCmplxDst,
-  uint32_t numSamples);
-
-  /**
-   * @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.
-   */
-
-  void arm_cmplx_mult_real_q31(
-  q31_t * pSrcCmplx,
-  q31_t * pSrcReal,
-  q31_t * pCmplxDst,
-  uint32_t numSamples);
-
-  /**
-   * @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);
-
-  /**
-   * @brief  Minimum value of a Q7 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *result is output pointer
-   * @param[in]  index is the array index of the minimum value in the input buffer.
-   * @return none.
-   */
-
-  void arm_min_q7(
-  q7_t * pSrc,
-  uint32_t blockSize,
-  q7_t * result,
-  uint32_t * index);
-
-  /**
-   * @brief  Minimum value of a Q15 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output pointer
-   * @param[in]  *pIndex is the array index of the minimum value in the input buffer.
-   * @return none.
-   */
-
-  void arm_min_q15(
-  q15_t * pSrc,
-  uint32_t blockSize,
-  q15_t * pResult,
-  uint32_t * pIndex);
-
-  /**
-   * @brief  Minimum value of a Q31 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output pointer
-   * @param[out]  *pIndex is the array index of the minimum value in the input buffer.
-   * @return none.
-   */
-  void arm_min_q31(
-  q31_t * pSrc,
-  uint32_t blockSize,
-  q31_t * pResult,
-  uint32_t * pIndex);
-
-  /**
-   * @brief  Minimum value of a floating-point vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @param[out]  *pResult is output pointer
-   * @param[out]  *pIndex is the array index of the minimum value in the input buffer.
-   * @return none.
-   */
-
-  void arm_min_f32(
-  float32_t * pSrc,
-  uint32_t blockSize,
-  float32_t * pResult,
-  uint32_t * pIndex);
-
-/**
- * @brief Maximum value of a Q7 vector.
- * @param[in]       *pSrc points to the input buffer
- * @param[in]       blockSize length of the input vector
- * @param[out]      *pResult maximum value returned here
- * @param[out]      *pIndex index of maximum value returned here
- * @return none.
- */
-
-  void arm_max_q7(
-  q7_t * pSrc,
-  uint32_t blockSize,
-  q7_t * pResult,
-  uint32_t * pIndex);
-
-/**
- * @brief Maximum value of a Q15 vector.
- * @param[in]       *pSrc points to the input buffer
- * @param[in]       blockSize length of the input vector
- * @param[out]      *pResult maximum value returned here
- * @param[out]      *pIndex index of maximum value returned here
- * @return none.
- */
-
-  void arm_max_q15(
-  q15_t * pSrc,
-  uint32_t blockSize,
-  q15_t * pResult,
-  uint32_t * pIndex);
-
-/**
- * @brief Maximum value of a Q31 vector.
- * @param[in]       *pSrc points to the input buffer
- * @param[in]       blockSize length of the input vector
- * @param[out]      *pResult maximum value returned here
- * @param[out]      *pIndex index of maximum value returned here
- * @return none.
- */
-
-  void arm_max_q31(
-  q31_t * pSrc,
-  uint32_t blockSize,
-  q31_t * pResult,
-  uint32_t * pIndex);
-
-/**
- * @brief Maximum value of a floating-point vector.
- * @param[in]       *pSrc points to the input buffer
- * @param[in]       blockSize length of the input vector
- * @param[out]      *pResult maximum value returned here
- * @param[out]      *pIndex index of maximum value returned here
- * @return none.
- */
-
-  void arm_max_f32(
-  float32_t * pSrc,
-  uint32_t blockSize,
-  float32_t * pResult,
-  uint32_t * pIndex);
-
-  /**
-   * @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.
-   */
-
-  void arm_cmplx_mult_cmplx_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  q15_t * pDst,
-  uint32_t numSamples);
-
-  /**
-   * @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.
-   */
-
-  void arm_cmplx_mult_cmplx_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  q31_t * pDst,
-  uint32_t numSamples);
-
-  /**
-   * @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);
-
-  /**
-   * @brief Converts the elements of the floating-point vector to Q31 vector.
-   * @param[in]       *pSrc points to the floating-point input vector
-   * @param[out]      *pDst points to the Q31 output vector
-   * @param[in]       blockSize length of the input vector
-   * @return none.
-   */
-  void arm_float_to_q31(
-  float32_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Converts the elements of the floating-point vector to Q15 vector.
-   * @param[in]       *pSrc points to the floating-point input vector
-   * @param[out]      *pDst points to the Q15 output vector
-   * @param[in]       blockSize length of the input vector
-   * @return          none
-   */
-  void arm_float_to_q15(
-  float32_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief Converts the elements of the floating-point vector to Q7 vector.
-   * @param[in]       *pSrc points to the floating-point input vector
-   * @param[out]      *pDst points to the Q7 output vector
-   * @param[in]       blockSize length of the input vector
-   * @return          none
-   */
-  void arm_float_to_q7(
-  float32_t * pSrc,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief  Converts the elements of the Q31 vector to Q15 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[out]  *pDst is output pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @return none.
-   */
-  void arm_q31_to_q15(
-  q31_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Converts the elements of the Q31 vector to Q7 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[out]  *pDst is output pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @return none.
-   */
-  void arm_q31_to_q7(
-  q31_t * pSrc,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-  /**
-   * @brief  Converts the elements of the Q15 vector to floating-point vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[out]  *pDst is output pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @return none.
-   */
-  void arm_q15_to_float(
-  q15_t * pSrc,
-  float32_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief  Converts the elements of the Q15 vector to Q31 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[out]  *pDst is output pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @return none.
-   */
-  void arm_q15_to_q31(
-  q15_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @brief  Converts the elements of the Q15 vector to Q7 vector.
-   * @param[in]  *pSrc is input pointer
-   * @param[out]  *pDst is output pointer
-   * @param[in]  blockSize is the number of samples to process
-   * @return none.
-   */
-  void arm_q15_to_q7(
-  q15_t * pSrc,
-  q7_t * pDst,
-  uint32_t blockSize);
-
-
-  /**
-   * @ingroup groupInterpolation
-   */
-
-  /**
-   * @defgroup BilinearInterpolate Bilinear Interpolation
-   *
-   * Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid.
-   * The underlying function <code>f(x, y)</code> is sampled on a regular grid and the interpolation process
-   * determines values between the grid points.
-   * Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension.
-   * Bilinear interpolation is often used in image processing to rescale images.
-   * The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types.
-   *
-   * <b>Algorithm</b>
-   * \par
-   * The instance structure used by the bilinear interpolation functions describes a two dimensional data table.
-   * For floating-point, the instance structure is defined as:
-   * <pre>
-   *   typedef struct
-   *   {
-   *     uint16_t numRows;
-   *     uint16_t numCols;
-   *     float32_t *pData;
-   * } arm_bilinear_interp_instance_f32;
-   * </pre>
-   *
-   * \par
-   * where <code>numRows</code> specifies the number of rows in the table;
-   * <code>numCols</code> specifies the number of columns in the table;
-   * and <code>pData</code> points to an array of size <code>numRows*numCols</code> values.
-   * The data table <code>pTable</code> is organized in row order and the supplied data values fall on integer indexes.
-   * That is, table element (x,y) is located at <code>pTable[x + y*numCols]</code> where x and y are integers.
-   *
-   * \par
-   * Let <code>(x, y)</code> specify the desired interpolation point.  Then define:
-   * <pre>
-   *     XF = floor(x)
-   *     YF = floor(y)
-   * </pre>
-   * \par
-   * The interpolated output point is computed as:
-   * <pre>
-   *  f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))
-   *           + f(XF+1, YF) * (x-XF)*(1-(y-YF))
-   *           + f(XF, YF+1) * (1-(x-XF))*(y-YF)
-   *           + f(XF+1, YF+1) * (x-XF)*(y-YF)
-   * </pre>
-   * Note that the coordinates (x, y) contain integer and fractional components.
-   * The integer components specify which portion of the table to use while the
-   * fractional components control the interpolation processor.
-   *
-   * \par
-   * if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output.
-   */
-
-  /**
-   * @addtogroup BilinearInterpolate
-   * @{
-   */
-
-  /**
-  *
-  * @brief  Floating-point bilinear interpolation.
-  * @param[in,out] *S points to an instance of the interpolation structure.
-  * @param[in] X interpolation coordinate.
-  * @param[in] Y interpolation coordinate.
-  * @return out interpolated value.
-  */
-
-
-  static __INLINE float32_t arm_bilinear_interp_f32(
-  const arm_bilinear_interp_instance_f32 * S,
-  float32_t X,
-  float32_t Y)
-  {
-    float32_t out;
-    float32_t f00, f01, f10, f11;
-    float32_t *pData = S->pData;
-    int32_t xIndex, yIndex, index;
-    float32_t xdiff, ydiff;
-    float32_t b1, b2, b3, b4;
-
-    xIndex = (int32_t) X;
-    yIndex = (int32_t) Y;
-
-    /* Care taken for table outside boundary */
-    /* Returns zero output when values are outside table boundary */
-    if(xIndex < 0 || xIndex > (S->numRows - 1) || yIndex < 0
-       || yIndex > (S->numCols - 1))
-    {
-      return (0);
-    }
-
-    /* Calculation of index for two nearest points in X-direction */
-    index = (xIndex - 1) + (yIndex - 1) * S->numCols;
-
-
-    /* Read two nearest points in X-direction */
-    f00 = pData[index];
-    f01 = pData[index + 1];
-
-    /* Calculation of index for two nearest points in Y-direction */
-    index = (xIndex - 1) + (yIndex) * S->numCols;
-
-
-    /* Read two nearest points in Y-direction */
-    f10 = pData[index];
-    f11 = pData[index + 1];
-
-    /* Calculation of intermediate values */
-    b1 = f00;
-    b2 = f01 - f00;
-    b3 = f10 - f00;
-    b4 = f00 - f01 - f10 + f11;
-
-    /* Calculation of fractional part in X */
-    xdiff = X - xIndex;
-
-    /* Calculation of fractional part in Y */
-    ydiff = Y - yIndex;
-
-    /* Calculation of bi-linear interpolated output */
-    out = b1 + b2 * xdiff + b3 * ydiff + b4 * xdiff * ydiff;
-
-    /* return to application */
-    return (out);
-
-  }
-
-  /**
-  *
-  * @brief  Q31 bilinear interpolation.
-  * @param[in,out] *S points to an instance of the interpolation structure.
-  * @param[in] X interpolation coordinate in 12.20 format.
-  * @param[in] Y interpolation coordinate in 12.20 format.
-  * @return out interpolated value.
-  */
-
-  static __INLINE q31_t arm_bilinear_interp_q31(
-  arm_bilinear_interp_instance_q31 * S,
-  q31_t X,
-  q31_t Y)
-  {
-    q31_t out;                                   /* Temporary output */
-    q31_t acc = 0;                               /* output */
-    q31_t xfract, yfract;                        /* X, Y fractional parts */
-    q31_t x1, x2, y1, y2;                        /* Nearest output values */
-    int32_t rI, cI;                              /* Row and column indices */
-    q31_t *pYData = S->pData;                    /* pointer to output table values */
-    uint32_t nCols = S->numCols;                 /* num of rows */
-
-
-    /* Input is in 12.20 format */
-    /* 12 bits for the table index */
-    /* Index value calculation */
-    rI = ((X & 0xFFF00000) >> 20u);
-
-    /* Input is in 12.20 format */
-    /* 12 bits for the table index */
-    /* Index value calculation */
-    cI = ((Y & 0xFFF00000) >> 20u);
-
-    /* Care taken for table outside boundary */
-    /* Returns zero output when values are outside table boundary */
-    if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
-    {
-      return (0);
-    }
-
-    /* 20 bits for the fractional part */
-    /* shift left xfract by 11 to keep 1.31 format */
-    xfract = (X & 0x000FFFFF) << 11u;
-
-    /* Read two nearest output values from the index */
-    x1 = pYData[(rI) + nCols * (cI)];
-    x2 = pYData[(rI) + nCols * (cI) + 1u];
-
-    /* 20 bits for the fractional part */
-    /* shift left yfract by 11 to keep 1.31 format */
-    yfract = (Y & 0x000FFFFF) << 11u;
-
-    /* Read two nearest output values from the index */
-    y1 = pYData[(rI) + nCols * (cI + 1)];
-    y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
-
-    /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 3.29(q29) format */
-    out = ((q31_t) (((q63_t) x1 * (0x7FFFFFFF - xfract)) >> 32));
-    acc = ((q31_t) (((q63_t) out * (0x7FFFFFFF - yfract)) >> 32));
-
-    /* x2 * (xfract) * (1-yfract)  in 3.29(q29) and adding to acc */
-    out = ((q31_t) ((q63_t) x2 * (0x7FFFFFFF - yfract) >> 32));
-    acc += ((q31_t) ((q63_t) out * (xfract) >> 32));
-
-    /* y1 * (1 - xfract) * (yfract)  in 3.29(q29) and adding to acc */
-    out = ((q31_t) ((q63_t) y1 * (0x7FFFFFFF - xfract) >> 32));
-    acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
-
-    /* y2 * (xfract) * (yfract)  in 3.29(q29) and adding to acc */
-    out = ((q31_t) ((q63_t) y2 * (xfract) >> 32));
-    acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
-
-    /* Convert acc to 1.31(q31) format */
-    return (acc << 2u);
-
-  }
-
-  /**
-  * @brief  Q15 bilinear interpolation.
-  * @param[in,out] *S points to an instance of the interpolation structure.
-  * @param[in] X interpolation coordinate in 12.20 format.
-  * @param[in] Y interpolation coordinate in 12.20 format.
-  * @return out interpolated value.
-  */
-
-  static __INLINE q15_t arm_bilinear_interp_q15(
-  arm_bilinear_interp_instance_q15 * S,
-  q31_t X,
-  q31_t Y)
-  {
-    q63_t acc = 0;                               /* output */
-    q31_t out;                                   /* Temporary output */
-    q15_t x1, x2, y1, y2;                        /* Nearest output values */
-    q31_t xfract, yfract;                        /* X, Y fractional parts */
-    int32_t rI, cI;                              /* Row and column indices */
-    q15_t *pYData = S->pData;                    /* pointer to output table values */
-    uint32_t nCols = S->numCols;                 /* num of rows */
-
-    /* Input is in 12.20 format */
-    /* 12 bits for the table index */
-    /* Index value calculation */
-    rI = ((X & 0xFFF00000) >> 20);
-
-    /* Input is in 12.20 format */
-    /* 12 bits for the table index */
-    /* Index value calculation */
-    cI = ((Y & 0xFFF00000) >> 20);
-
-    /* Care taken for table outside boundary */
-    /* Returns zero output when values are outside table boundary */
-    if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
-    {
-      return (0);
-    }
-
-    /* 20 bits for the fractional part */
-    /* xfract should be in 12.20 format */
-    xfract = (X & 0x000FFFFF);
-
-    /* Read two nearest output values from the index */
-    x1 = pYData[(rI) + nCols * (cI)];
-    x2 = pYData[(rI) + nCols * (cI) + 1u];
-
-
-    /* 20 bits for the fractional part */
-    /* yfract should be in 12.20 format */
-    yfract = (Y & 0x000FFFFF);
-
-    /* Read two nearest output values from the index */
-    y1 = pYData[(rI) + nCols * (cI + 1)];
-    y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
-
-    /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 13.51 format */
-
-    /* x1 is in 1.15(q15), xfract in 12.20 format and out is in 13.35 format */
-    /* convert 13.35 to 13.31 by right shifting  and out is in 1.31 */
-    out = (q31_t) (((q63_t) x1 * (0xFFFFF - xfract)) >> 4u);
-    acc = ((q63_t) out * (0xFFFFF - yfract));
-
-    /* x2 * (xfract) * (1-yfract)  in 1.51 and adding to acc */
-    out = (q31_t) (((q63_t) x2 * (0xFFFFF - yfract)) >> 4u);
-    acc += ((q63_t) out * (xfract));
-
-    /* y1 * (1 - xfract) * (yfract)  in 1.51 and adding to acc */
-    out = (q31_t) (((q63_t) y1 * (0xFFFFF - xfract)) >> 4u);
-    acc += ((q63_t) out * (yfract));
-
-    /* y2 * (xfract) * (yfract)  in 1.51 and adding to acc */
-    out = (q31_t) (((q63_t) y2 * (xfract)) >> 4u);
-    acc += ((q63_t) out * (yfract));
-
-    /* acc is in 13.51 format and down shift acc by 36 times */
-    /* Convert out to 1.15 format */
-    return (acc >> 36);
-
-  }
-
-  /**
-  * @brief  Q7 bilinear interpolation.
-  * @param[in,out] *S points to an instance of the interpolation structure.
-  * @param[in] X interpolation coordinate in 12.20 format.
-  * @param[in] Y interpolation coordinate in 12.20 format.
-  * @return out interpolated value.
-  */
-
-  static __INLINE q7_t arm_bilinear_interp_q7(
-  arm_bilinear_interp_instance_q7 * S,
-  q31_t X,
-  q31_t Y)
-  {
-    q63_t acc = 0;                               /* output */
-    q31_t out;                                   /* Temporary output */
-    q31_t xfract, yfract;                        /* X, Y fractional parts */
-    q7_t x1, x2, y1, y2;                         /* Nearest output values */
-    int32_t rI, cI;                              /* Row and column indices */
-    q7_t *pYData = S->pData;                     /* pointer to output table values */
-    uint32_t nCols = S->numCols;                 /* num of rows */
-
-    /* Input is in 12.20 format */
-    /* 12 bits for the table index */
-    /* Index value calculation */
-    rI = ((X & 0xFFF00000) >> 20);
-
-    /* Input is in 12.20 format */
-    /* 12 bits for the table index */
-    /* Index value calculation */
-    cI = ((Y & 0xFFF00000) >> 20);
-
-    /* Care taken for table outside boundary */
-    /* Returns zero output when values are outside table boundary */
-    if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
-    {
-      return (0);
-    }
-
-    /* 20 bits for the fractional part */
-    /* xfract should be in 12.20 format */
-    xfract = (X & 0x000FFFFF);
-
-    /* Read two nearest output values from the index */
-    x1 = pYData[(rI) + nCols * (cI)];
-    x2 = pYData[(rI) + nCols * (cI) + 1u];
-
-
-    /* 20 bits for the fractional part */
-    /* yfract should be in 12.20 format */
-    yfract = (Y & 0x000FFFFF);
-
-    /* Read two nearest output values from the index */
-    y1 = pYData[(rI) + nCols * (cI + 1)];
-    y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
-
-    /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 16.47 format */
-    out = ((x1 * (0xFFFFF - xfract)));
-    acc = (((q63_t) out * (0xFFFFF - yfract)));
-
-    /* x2 * (xfract) * (1-yfract)  in 2.22 and adding to acc */
-    out = ((x2 * (0xFFFFF - yfract)));
-    acc += (((q63_t) out * (xfract)));
-
-    /* y1 * (1 - xfract) * (yfract)  in 2.22 and adding to acc */
-    out = ((y1 * (0xFFFFF - xfract)));
-    acc += (((q63_t) out * (yfract)));
-
-    /* y2 * (xfract) * (yfract)  in 2.22 and adding to acc */
-    out = ((y2 * (yfract)));
-    acc += (((q63_t) out * (xfract)));
-
-    /* acc in 16.47 format and down shift by 40 to convert to 1.7 format */
-    return (acc >> 40);
-
-  }
-
-  /**
-   * @} end of BilinearInterpolate group
-   */
-
-
-#if   defined ( __CC_ARM ) //Keil
-//SMMLAR
-  #define multAcc_32x32_keep32_R(a, x, y) \
-  a = (q31_t) (((((q63_t) a) << 32) + ((q63_t) x * y) + 0x80000000LL ) >> 32)
-
-//SMMLSR
-  #define multSub_32x32_keep32_R(a, x, y) \
-  a = (q31_t) (((((q63_t) a) << 32) - ((q63_t) x * y) + 0x80000000LL ) >> 32)
-
-//SMMULR
-  #define mult_32x32_keep32_R(a, x, y) \
-  a = (q31_t) (((q63_t) x * y + 0x80000000LL ) >> 32)
-
-//Enter low optimization region - place directly above function definition
-  #define LOW_OPTIMIZATION_ENTER \
-     _Pragma ("push")         \
-     _Pragma ("O1")
-
-//Exit low optimization region - place directly after end of function definition
-  #define LOW_OPTIMIZATION_EXIT \
-     _Pragma ("pop")
-
-//Enter low optimization region - place directly above function definition
-  #define IAR_ONLY_LOW_OPTIMIZATION_ENTER
-
-//Exit low optimization region - place directly after end of function definition
-  #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
-
-#elif defined(__ICCARM__) //IAR
- //SMMLA
-  #define multAcc_32x32_keep32_R(a, x, y) \
-  a += (q31_t) (((q63_t) x * y) >> 32)
-
- //SMMLS
-  #define multSub_32x32_keep32_R(a, x, y) \
-  a -= (q31_t) (((q63_t) x * y) >> 32)
-
-//SMMUL
-  #define mult_32x32_keep32_R(a, x, y) \
-  a = (q31_t) (((q63_t) x * y ) >> 32)
-
-//Enter low optimization region - place directly above function definition
-  #define LOW_OPTIMIZATION_ENTER \
-     _Pragma ("optimize=low")
-
-//Exit low optimization region - place directly after end of function definition
-  #define LOW_OPTIMIZATION_EXIT
-
-//Enter low optimization region - place directly above function definition
-  #define IAR_ONLY_LOW_OPTIMIZATION_ENTER \
-     _Pragma ("optimize=low")
-
-//Exit low optimization region - place directly after end of function definition
-  #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
-
-#elif defined(__GNUC__)
- //SMMLA
-  #define multAcc_32x32_keep32_R(a, x, y) \
-  a += (q31_t) (((q63_t) x * y) >> 32)
-
- //SMMLS
-  #define multSub_32x32_keep32_R(a, x, y) \
-  a -= (q31_t) (((q63_t) x * y) >> 32)
-
-//SMMUL
-  #define mult_32x32_keep32_R(a, x, y) \
-  a = (q31_t) (((q63_t) x * y ) >> 32)
-
-  #define LOW_OPTIMIZATION_ENTER __attribute__(( optimize("-O1") ))
-
-  #define LOW_OPTIMIZATION_EXIT
-
-  #define IAR_ONLY_LOW_OPTIMIZATION_ENTER
-
-  #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
-
-#endif
-
-
-
-
-
-#ifdef	__cplusplus
-}
-#endif
-
-
-#endif /* _ARM_MATH_H */
-
-
-/**
- *
- * End of file.
- */
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/modules/mathlib/CMSIS/Include/core_cm3.h
deleted file mode 100644
index 8ac6dc078..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cm3.h
+++ /dev/null
@@ -1,1627 +0,0 @@
-/**************************************************************************//**
- * @file     core_cm3.h
- * @brief    CMSIS Cortex-M3 Core Peripheral Access Layer Header File
- * @version  V3.20
- * @date     25. February 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
-   All rights reserved.
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-   - Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   - 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.
-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
-   ---------------------------------------------------------------------------*/
-
-
-#if defined ( __ICCARM__ )
- #pragma system_include  /* treat file as system include file for MISRA check */
-#endif
-
-#ifdef __cplusplus
- extern "C" {
-#endif
-
-#ifndef __CORE_CM3_H_GENERIC
-#define __CORE_CM3_H_GENERIC
-
-/** \page CMSIS_MISRA_Exceptions  MISRA-C:2004 Compliance Exceptions
-  CMSIS violates the following MISRA-C:2004 rules:
-
-   \li Required Rule 8.5, object/function definition in header file.<br>
-     Function definitions in header files are used to allow 'inlining'.
-
-   \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
-     Unions are used for effective representation of core registers.
-
-   \li Advisory Rule 19.7, Function-like macro defined.<br>
-     Function-like macros are used to allow more efficient code.
- */
-
-
-/*******************************************************************************
- *                 CMSIS definitions
- ******************************************************************************/
-/** \ingroup Cortex_M3
-  @{
- */
-
-/*  CMSIS CM3 definitions */
-#define __CM3_CMSIS_VERSION_MAIN  (0x03)                                   /*!< [31:16] CMSIS HAL main version   */
-#define __CM3_CMSIS_VERSION_SUB   (0x20)                                   /*!< [15:0]  CMSIS HAL sub version    */
-#define __CM3_CMSIS_VERSION       ((__CM3_CMSIS_VERSION_MAIN << 16) | \
-                                    __CM3_CMSIS_VERSION_SUB          )     /*!< CMSIS HAL version number         */
-
-#define __CORTEX_M                (0x03)                                   /*!< Cortex-M Core                    */
-
-
-#if   defined ( __CC_ARM )
-  #define __ASM            __asm                                      /*!< asm keyword for ARM Compiler          */
-  #define __INLINE         __inline                                   /*!< inline keyword for ARM Compiler       */
-  #define __STATIC_INLINE  static __inline
-
-#elif defined ( __ICCARM__ )
-  #define __ASM            __asm                                      /*!< asm keyword for IAR Compiler          */
-  #define __INLINE         inline                                     /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */
-  #define __STATIC_INLINE  static inline
-
-#elif defined ( __TMS470__ )
-  #define __ASM            __asm                                      /*!< asm keyword for TI CCS Compiler       */
-  #define __STATIC_INLINE  static inline
-
-#elif defined ( __GNUC__ )
-  #define __ASM            __asm                                      /*!< asm keyword for GNU Compiler          */
-  #define __INLINE         inline                                     /*!< inline keyword for GNU Compiler       */
-  #define __STATIC_INLINE  static inline
-
-#elif defined ( __TASKING__ )
-  #define __ASM            __asm                                      /*!< asm keyword for TASKING Compiler      */
-  #define __INLINE         inline                                     /*!< inline keyword for TASKING Compiler   */
-  #define __STATIC_INLINE  static inline
-
-#endif
-
-/** __FPU_USED indicates whether an FPU is used or not. This core does not support an FPU at all
-*/
-#define __FPU_USED       0
-
-#if defined ( __CC_ARM )
-  #if defined __TARGET_FPU_VFP
-    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-  #endif
-
-#elif defined ( __ICCARM__ )
-  #if defined __ARMVFP__
-    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-  #endif
-
-#elif defined ( __TMS470__ )
-  #if defined __TI__VFP_SUPPORT____
-    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-  #endif
-
-#elif defined ( __GNUC__ )
-  #if defined (__VFP_FP__) && !defined(__SOFTFP__)
-    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-  #endif
-
-#elif defined ( __TASKING__ )
-  #if defined __FPU_VFP__
-    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-  #endif
-#endif
-
-#include <stdint.h>                      /* standard types definitions                      */
-#include "core_cmInstr.h"                /* Core Instruction Access                         */
-#include "core_cmFunc.h"                 /* Core Function Access                            */
-
-#endif /* __CORE_CM3_H_GENERIC */
-
-#ifndef __CMSIS_GENERIC
-
-#ifndef __CORE_CM3_H_DEPENDANT
-#define __CORE_CM3_H_DEPENDANT
-
-/* check device defines and use defaults */
-#if defined __CHECK_DEVICE_DEFINES
-  #ifndef __CM3_REV
-    #define __CM3_REV               0x0200
-    #warning "__CM3_REV not defined in device header file; using default!"
-  #endif
-
-  #ifndef __MPU_PRESENT
-    #define __MPU_PRESENT             0
-    #warning "__MPU_PRESENT not defined in device header file; using default!"
-  #endif
-
-  #ifndef __NVIC_PRIO_BITS
-    #define __NVIC_PRIO_BITS          4
-    #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
-  #endif
-
-  #ifndef __Vendor_SysTickConfig
-    #define __Vendor_SysTickConfig    0
-    #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
-  #endif
-#endif
-
-/* IO definitions (access restrictions to peripheral registers) */
-/**
-    \defgroup CMSIS_glob_defs CMSIS Global Defines
-
-    <strong>IO Type Qualifiers</strong> are used
-    \li to specify the access to peripheral variables.
-    \li for automatic generation of peripheral register debug information.
-*/
-#ifdef __cplusplus
-  #define   __I     volatile             /*!< Defines 'read only' permissions                 */
-#else
-  #define   __I     volatile const       /*!< Defines 'read only' permissions                 */
-#endif
-#define     __O     volatile             /*!< Defines 'write only' permissions                */
-#define     __IO    volatile             /*!< Defines 'read / write' permissions              */
-
-/*@} end of group Cortex_M3 */
-
-
-
-/*******************************************************************************
- *                 Register Abstraction
-  Core Register contain:
-  - Core Register
-  - Core NVIC Register
-  - Core SCB Register
-  - Core SysTick Register
-  - Core Debug Register
-  - Core MPU Register
- ******************************************************************************/
-/** \defgroup CMSIS_core_register Defines and Type Definitions
-    \brief Type definitions and defines for Cortex-M processor based devices.
-*/
-
-/** \ingroup    CMSIS_core_register
-    \defgroup   CMSIS_CORE  Status and Control Registers
-    \brief  Core Register type definitions.
-  @{
- */
-
-/** \brief  Union type to access the Application Program Status Register (APSR).
- */
-typedef union
-{
-  struct
-  {
-#if (__CORTEX_M != 0x04)
-    uint32_t _reserved0:27;              /*!< bit:  0..26  Reserved                           */
-#else
-    uint32_t _reserved0:16;              /*!< bit:  0..15  Reserved                           */
-    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */
-    uint32_t _reserved1:7;               /*!< bit: 20..26  Reserved                           */
-#endif
-    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */
-    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */
-    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */
-    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */
-    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */
-  } b;                                   /*!< Structure used for bit  access                  */
-  uint32_t w;                            /*!< Type      used for word access                  */
-} APSR_Type;
-
-
-/** \brief  Union type to access the Interrupt Program Status Register (IPSR).
- */
-typedef union
-{
-  struct
-  {
-    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */
-    uint32_t _reserved0:23;              /*!< bit:  9..31  Reserved                           */
-  } b;                                   /*!< Structure used for bit  access                  */
-  uint32_t w;                            /*!< Type      used for word access                  */
-} IPSR_Type;
-
-
-/** \brief  Union type to access the Special-Purpose Program Status Registers (xPSR).
- */
-typedef union
-{
-  struct
-  {
-    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */
-#if (__CORTEX_M != 0x04)
-    uint32_t _reserved0:15;              /*!< bit:  9..23  Reserved                           */
-#else
-    uint32_t _reserved0:7;               /*!< bit:  9..15  Reserved                           */
-    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */
-    uint32_t _reserved1:4;               /*!< bit: 20..23  Reserved                           */
-#endif
-    uint32_t T:1;                        /*!< bit:     24  Thumb bit        (read 0)          */
-    uint32_t IT:2;                       /*!< bit: 25..26  saved IT state   (read 0)          */
-    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */
-    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */
-    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */
-    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */
-    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */
-  } b;                                   /*!< Structure used for bit  access                  */
-  uint32_t w;                            /*!< Type      used for word access                  */
-} xPSR_Type;
-
-
-/** \brief  Union type to access the Control Registers (CONTROL).
- */
-typedef union
-{
-  struct
-  {
-    uint32_t nPRIV:1;                    /*!< bit:      0  Execution privilege in Thread mode */
-    uint32_t SPSEL:1;                    /*!< bit:      1  Stack to be used                   */
-    uint32_t FPCA:1;                     /*!< bit:      2  FP extension active flag           */
-    uint32_t _reserved0:29;              /*!< bit:  3..31  Reserved                           */
-  } b;                                   /*!< Structure used for bit  access                  */
-  uint32_t w;                            /*!< Type      used for word access                  */
-} CONTROL_Type;
-
-/*@} end of group CMSIS_CORE */
-
-
-/** \ingroup    CMSIS_core_register
-    \defgroup   CMSIS_NVIC  Nested Vectored Interrupt Controller (NVIC)
-    \brief      Type definitions for the NVIC Registers
-  @{
- */
-
-/** \brief  Structure type to access the Nested Vectored Interrupt Controller (NVIC).
- */
-typedef struct
-{
-  __IO uint32_t ISER[8];                 /*!< Offset: 0x000 (R/W)  Interrupt Set Enable Register           */
-       uint32_t RESERVED0[24];
-  __IO uint32_t ICER[8];                 /*!< Offset: 0x080 (R/W)  Interrupt Clear Enable Register         */
-       uint32_t RSERVED1[24];
-  __IO uint32_t ISPR[8];                 /*!< Offset: 0x100 (R/W)  Interrupt Set Pending Register          */
-       uint32_t RESERVED2[24];
-  __IO uint32_t ICPR[8];                 /*!< Offset: 0x180 (R/W)  Interrupt Clear Pending Register        */
-       uint32_t RESERVED3[24];
-  __IO uint32_t IABR[8];                 /*!< Offset: 0x200 (R/W)  Interrupt Active bit Register           */
-       uint32_t RESERVED4[56];
-  __IO uint8_t  IP[240];                 /*!< Offset: 0x300 (R/W)  Interrupt Priority Register (8Bit wide) */
-       uint32_t RESERVED5[644];
-  __O  uint32_t STIR;                    /*!< Offset: 0xE00 ( /W)  Software Trigger Interrupt Register     */
-}  NVIC_Type;
-
-/* Software Triggered Interrupt Register Definitions */
-#define NVIC_STIR_INTID_Pos                 0                                          /*!< STIR: INTLINESNUM Position */
-#define NVIC_STIR_INTID_Msk                (0x1FFUL << NVIC_STIR_INTID_Pos)            /*!< STIR: INTLINESNUM Mask */
-
-/*@} end of group CMSIS_NVIC */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_SCB     System Control Block (SCB)
-    \brief      Type definitions for the System Control Block Registers
-  @{
- */
-
-/** \brief  Structure type to access the System Control Block (SCB).
- */
-typedef struct
-{
-  __I  uint32_t CPUID;                   /*!< Offset: 0x000 (R/ )  CPUID Base Register                                   */
-  __IO uint32_t ICSR;                    /*!< Offset: 0x004 (R/W)  Interrupt Control and State Register                  */
-  __IO uint32_t VTOR;                    /*!< Offset: 0x008 (R/W)  Vector Table Offset Register                          */
-  __IO uint32_t AIRCR;                   /*!< Offset: 0x00C (R/W)  Application Interrupt and Reset Control Register      */
-  __IO uint32_t SCR;                     /*!< Offset: 0x010 (R/W)  System Control Register                               */
-  __IO uint32_t CCR;                     /*!< Offset: 0x014 (R/W)  Configuration Control Register                        */
-  __IO uint8_t  SHP[12];                 /*!< Offset: 0x018 (R/W)  System Handlers Priority Registers (4-7, 8-11, 12-15) */
-  __IO uint32_t SHCSR;                   /*!< Offset: 0x024 (R/W)  System Handler Control and State Register             */
-  __IO uint32_t CFSR;                    /*!< Offset: 0x028 (R/W)  Configurable Fault Status Register                    */
-  __IO uint32_t HFSR;                    /*!< Offset: 0x02C (R/W)  HardFault Status Register                             */
-  __IO uint32_t DFSR;                    /*!< Offset: 0x030 (R/W)  Debug Fault Status Register                           */
-  __IO uint32_t MMFAR;                   /*!< Offset: 0x034 (R/W)  MemManage Fault Address Register                      */
-  __IO uint32_t BFAR;                    /*!< Offset: 0x038 (R/W)  BusFault Address Register                             */
-  __IO uint32_t AFSR;                    /*!< Offset: 0x03C (R/W)  Auxiliary Fault Status Register                       */
-  __I  uint32_t PFR[2];                  /*!< Offset: 0x040 (R/ )  Processor Feature Register                            */
-  __I  uint32_t DFR;                     /*!< Offset: 0x048 (R/ )  Debug Feature Register                                */
-  __I  uint32_t ADR;                     /*!< Offset: 0x04C (R/ )  Auxiliary Feature Register                            */
-  __I  uint32_t MMFR[4];                 /*!< Offset: 0x050 (R/ )  Memory Model Feature Register                         */
-  __I  uint32_t ISAR[5];                 /*!< Offset: 0x060 (R/ )  Instruction Set Attributes Register                   */
-       uint32_t RESERVED0[5];
-  __IO uint32_t CPACR;                   /*!< Offset: 0x088 (R/W)  Coprocessor Access Control Register                   */
-} SCB_Type;
-
-/* SCB CPUID Register Definitions */
-#define SCB_CPUID_IMPLEMENTER_Pos          24                                             /*!< SCB CPUID: IMPLEMENTER Position */
-#define SCB_CPUID_IMPLEMENTER_Msk          (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos)          /*!< SCB CPUID: IMPLEMENTER Mask */
-
-#define SCB_CPUID_VARIANT_Pos              20                                             /*!< SCB CPUID: VARIANT Position */
-#define SCB_CPUID_VARIANT_Msk              (0xFUL << SCB_CPUID_VARIANT_Pos)               /*!< SCB CPUID: VARIANT Mask */
-
-#define SCB_CPUID_ARCHITECTURE_Pos         16                                             /*!< SCB CPUID: ARCHITECTURE Position */
-#define SCB_CPUID_ARCHITECTURE_Msk         (0xFUL << SCB_CPUID_ARCHITECTURE_Pos)          /*!< SCB CPUID: ARCHITECTURE Mask */
-
-#define SCB_CPUID_PARTNO_Pos                4                                             /*!< SCB CPUID: PARTNO Position */
-#define SCB_CPUID_PARTNO_Msk               (0xFFFUL << SCB_CPUID_PARTNO_Pos)              /*!< SCB CPUID: PARTNO Mask */
-
-#define SCB_CPUID_REVISION_Pos              0                                             /*!< SCB CPUID: REVISION Position */
-#define SCB_CPUID_REVISION_Msk             (0xFUL << SCB_CPUID_REVISION_Pos)              /*!< SCB CPUID: REVISION Mask */
-
-/* SCB Interrupt Control State Register Definitions */
-#define SCB_ICSR_NMIPENDSET_Pos            31                                             /*!< SCB ICSR: NMIPENDSET Position */
-#define SCB_ICSR_NMIPENDSET_Msk            (1UL << SCB_ICSR_NMIPENDSET_Pos)               /*!< SCB ICSR: NMIPENDSET Mask */
-
-#define SCB_ICSR_PENDSVSET_Pos             28                                             /*!< SCB ICSR: PENDSVSET Position */
-#define SCB_ICSR_PENDSVSET_Msk             (1UL << SCB_ICSR_PENDSVSET_Pos)                /*!< SCB ICSR: PENDSVSET Mask */
-
-#define SCB_ICSR_PENDSVCLR_Pos             27                                             /*!< SCB ICSR: PENDSVCLR Position */
-#define SCB_ICSR_PENDSVCLR_Msk             (1UL << SCB_ICSR_PENDSVCLR_Pos)                /*!< SCB ICSR: PENDSVCLR Mask */
-
-#define SCB_ICSR_PENDSTSET_Pos             26                                             /*!< SCB ICSR: PENDSTSET Position */
-#define SCB_ICSR_PENDSTSET_Msk             (1UL << SCB_ICSR_PENDSTSET_Pos)                /*!< SCB ICSR: PENDSTSET Mask */
-
-#define SCB_ICSR_PENDSTCLR_Pos             25                                             /*!< SCB ICSR: PENDSTCLR Position */
-#define SCB_ICSR_PENDSTCLR_Msk             (1UL << SCB_ICSR_PENDSTCLR_Pos)                /*!< SCB ICSR: PENDSTCLR Mask */
-
-#define SCB_ICSR_ISRPREEMPT_Pos            23                                             /*!< SCB ICSR: ISRPREEMPT Position */
-#define SCB_ICSR_ISRPREEMPT_Msk            (1UL << SCB_ICSR_ISRPREEMPT_Pos)               /*!< SCB ICSR: ISRPREEMPT Mask */
-
-#define SCB_ICSR_ISRPENDING_Pos            22                                             /*!< SCB ICSR: ISRPENDING Position */
-#define SCB_ICSR_ISRPENDING_Msk            (1UL << SCB_ICSR_ISRPENDING_Pos)               /*!< SCB ICSR: ISRPENDING Mask */
-
-#define SCB_ICSR_VECTPENDING_Pos           12                                             /*!< SCB ICSR: VECTPENDING Position */
-#define SCB_ICSR_VECTPENDING_Msk           (0x1FFUL << SCB_ICSR_VECTPENDING_Pos)          /*!< SCB ICSR: VECTPENDING Mask */
-
-#define SCB_ICSR_RETTOBASE_Pos             11                                             /*!< SCB ICSR: RETTOBASE Position */
-#define SCB_ICSR_RETTOBASE_Msk             (1UL << SCB_ICSR_RETTOBASE_Pos)                /*!< SCB ICSR: RETTOBASE Mask */
-
-#define SCB_ICSR_VECTACTIVE_Pos             0                                             /*!< SCB ICSR: VECTACTIVE Position */
-#define SCB_ICSR_VECTACTIVE_Msk            (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos)           /*!< SCB ICSR: VECTACTIVE Mask */
-
-/* SCB Vector Table Offset Register Definitions */
-#if (__CM3_REV < 0x0201)                   /* core r2p1 */
-#define SCB_VTOR_TBLBASE_Pos               29                                             /*!< SCB VTOR: TBLBASE Position */
-#define SCB_VTOR_TBLBASE_Msk               (1UL << SCB_VTOR_TBLBASE_Pos)                  /*!< SCB VTOR: TBLBASE Mask */
-
-#define SCB_VTOR_TBLOFF_Pos                 7                                             /*!< SCB VTOR: TBLOFF Position */
-#define SCB_VTOR_TBLOFF_Msk                (0x3FFFFFUL << SCB_VTOR_TBLOFF_Pos)            /*!< SCB VTOR: TBLOFF Mask */
-#else
-#define SCB_VTOR_TBLOFF_Pos                 7                                             /*!< SCB VTOR: TBLOFF Position */
-#define SCB_VTOR_TBLOFF_Msk                (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos)           /*!< SCB VTOR: TBLOFF Mask */
-#endif
-
-/* SCB Application Interrupt and Reset Control Register Definitions */
-#define SCB_AIRCR_VECTKEY_Pos              16                                             /*!< SCB AIRCR: VECTKEY Position */
-#define SCB_AIRCR_VECTKEY_Msk              (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos)            /*!< SCB AIRCR: VECTKEY Mask */
-
-#define SCB_AIRCR_VECTKEYSTAT_Pos          16                                             /*!< SCB AIRCR: VECTKEYSTAT Position */
-#define SCB_AIRCR_VECTKEYSTAT_Msk          (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos)        /*!< SCB AIRCR: VECTKEYSTAT Mask */
-
-#define SCB_AIRCR_ENDIANESS_Pos            15                                             /*!< SCB AIRCR: ENDIANESS Position */
-#define SCB_AIRCR_ENDIANESS_Msk            (1UL << SCB_AIRCR_ENDIANESS_Pos)               /*!< SCB AIRCR: ENDIANESS Mask */
-
-#define SCB_AIRCR_PRIGROUP_Pos              8                                             /*!< SCB AIRCR: PRIGROUP Position */
-#define SCB_AIRCR_PRIGROUP_Msk             (7UL << SCB_AIRCR_PRIGROUP_Pos)                /*!< SCB AIRCR: PRIGROUP Mask */
-
-#define SCB_AIRCR_SYSRESETREQ_Pos           2                                             /*!< SCB AIRCR: SYSRESETREQ Position */
-#define SCB_AIRCR_SYSRESETREQ_Msk          (1UL << SCB_AIRCR_SYSRESETREQ_Pos)             /*!< SCB AIRCR: SYSRESETREQ Mask */
-
-#define SCB_AIRCR_VECTCLRACTIVE_Pos         1                                             /*!< SCB AIRCR: VECTCLRACTIVE Position */
-#define SCB_AIRCR_VECTCLRACTIVE_Msk        (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos)           /*!< SCB AIRCR: VECTCLRACTIVE Mask */
-
-#define SCB_AIRCR_VECTRESET_Pos             0                                             /*!< SCB AIRCR: VECTRESET Position */
-#define SCB_AIRCR_VECTRESET_Msk            (1UL << SCB_AIRCR_VECTRESET_Pos)               /*!< SCB AIRCR: VECTRESET Mask */
-
-/* SCB System Control Register Definitions */
-#define SCB_SCR_SEVONPEND_Pos               4                                             /*!< SCB SCR: SEVONPEND Position */
-#define SCB_SCR_SEVONPEND_Msk              (1UL << SCB_SCR_SEVONPEND_Pos)                 /*!< SCB SCR: SEVONPEND Mask */
-
-#define SCB_SCR_SLEEPDEEP_Pos               2                                             /*!< SCB SCR: SLEEPDEEP Position */
-#define SCB_SCR_SLEEPDEEP_Msk              (1UL << SCB_SCR_SLEEPDEEP_Pos)                 /*!< SCB SCR: SLEEPDEEP Mask */
-
-#define SCB_SCR_SLEEPONEXIT_Pos             1                                             /*!< SCB SCR: SLEEPONEXIT Position */
-#define SCB_SCR_SLEEPONEXIT_Msk            (1UL << SCB_SCR_SLEEPONEXIT_Pos)               /*!< SCB SCR: SLEEPONEXIT Mask */
-
-/* SCB Configuration Control Register Definitions */
-#define SCB_CCR_STKALIGN_Pos                9                                             /*!< SCB CCR: STKALIGN Position */
-#define SCB_CCR_STKALIGN_Msk               (1UL << SCB_CCR_STKALIGN_Pos)                  /*!< SCB CCR: STKALIGN Mask */
-
-#define SCB_CCR_BFHFNMIGN_Pos               8                                             /*!< SCB CCR: BFHFNMIGN Position */
-#define SCB_CCR_BFHFNMIGN_Msk              (1UL << SCB_CCR_BFHFNMIGN_Pos)                 /*!< SCB CCR: BFHFNMIGN Mask */
-
-#define SCB_CCR_DIV_0_TRP_Pos               4                                             /*!< SCB CCR: DIV_0_TRP Position */
-#define SCB_CCR_DIV_0_TRP_Msk              (1UL << SCB_CCR_DIV_0_TRP_Pos)                 /*!< SCB CCR: DIV_0_TRP Mask */
-
-#define SCB_CCR_UNALIGN_TRP_Pos             3                                             /*!< SCB CCR: UNALIGN_TRP Position */
-#define SCB_CCR_UNALIGN_TRP_Msk            (1UL << SCB_CCR_UNALIGN_TRP_Pos)               /*!< SCB CCR: UNALIGN_TRP Mask */
-
-#define SCB_CCR_USERSETMPEND_Pos            1                                             /*!< SCB CCR: USERSETMPEND Position */
-#define SCB_CCR_USERSETMPEND_Msk           (1UL << SCB_CCR_USERSETMPEND_Pos)              /*!< SCB CCR: USERSETMPEND Mask */
-
-#define SCB_CCR_NONBASETHRDENA_Pos          0                                             /*!< SCB CCR: NONBASETHRDENA Position */
-#define SCB_CCR_NONBASETHRDENA_Msk         (1UL << SCB_CCR_NONBASETHRDENA_Pos)            /*!< SCB CCR: NONBASETHRDENA Mask */
-
-/* SCB System Handler Control and State Register Definitions */
-#define SCB_SHCSR_USGFAULTENA_Pos          18                                             /*!< SCB SHCSR: USGFAULTENA Position */
-#define SCB_SHCSR_USGFAULTENA_Msk          (1UL << SCB_SHCSR_USGFAULTENA_Pos)             /*!< SCB SHCSR: USGFAULTENA Mask */
-
-#define SCB_SHCSR_BUSFAULTENA_Pos          17                                             /*!< SCB SHCSR: BUSFAULTENA Position */
-#define SCB_SHCSR_BUSFAULTENA_Msk          (1UL << SCB_SHCSR_BUSFAULTENA_Pos)             /*!< SCB SHCSR: BUSFAULTENA Mask */
-
-#define SCB_SHCSR_MEMFAULTENA_Pos          16                                             /*!< SCB SHCSR: MEMFAULTENA Position */
-#define SCB_SHCSR_MEMFAULTENA_Msk          (1UL << SCB_SHCSR_MEMFAULTENA_Pos)             /*!< SCB SHCSR: MEMFAULTENA Mask */
-
-#define SCB_SHCSR_SVCALLPENDED_Pos         15                                             /*!< SCB SHCSR: SVCALLPENDED Position */
-#define SCB_SHCSR_SVCALLPENDED_Msk         (1UL << SCB_SHCSR_SVCALLPENDED_Pos)            /*!< SCB SHCSR: SVCALLPENDED Mask */
-
-#define SCB_SHCSR_BUSFAULTPENDED_Pos       14                                             /*!< SCB SHCSR: BUSFAULTPENDED Position */
-#define SCB_SHCSR_BUSFAULTPENDED_Msk       (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos)          /*!< SCB SHCSR: BUSFAULTPENDED Mask */
-
-#define SCB_SHCSR_MEMFAULTPENDED_Pos       13                                             /*!< SCB SHCSR: MEMFAULTPENDED Position */
-#define SCB_SHCSR_MEMFAULTPENDED_Msk       (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos)          /*!< SCB SHCSR: MEMFAULTPENDED Mask */
-
-#define SCB_SHCSR_USGFAULTPENDED_Pos       12                                             /*!< SCB SHCSR: USGFAULTPENDED Position */
-#define SCB_SHCSR_USGFAULTPENDED_Msk       (1UL << SCB_SHCSR_USGFAULTPENDED_Pos)          /*!< SCB SHCSR: USGFAULTPENDED Mask */
-
-#define SCB_SHCSR_SYSTICKACT_Pos           11                                             /*!< SCB SHCSR: SYSTICKACT Position */
-#define SCB_SHCSR_SYSTICKACT_Msk           (1UL << SCB_SHCSR_SYSTICKACT_Pos)              /*!< SCB SHCSR: SYSTICKACT Mask */
-
-#define SCB_SHCSR_PENDSVACT_Pos            10                                             /*!< SCB SHCSR: PENDSVACT Position */
-#define SCB_SHCSR_PENDSVACT_Msk            (1UL << SCB_SHCSR_PENDSVACT_Pos)               /*!< SCB SHCSR: PENDSVACT Mask */
-
-#define SCB_SHCSR_MONITORACT_Pos            8                                             /*!< SCB SHCSR: MONITORACT Position */
-#define SCB_SHCSR_MONITORACT_Msk           (1UL << SCB_SHCSR_MONITORACT_Pos)              /*!< SCB SHCSR: MONITORACT Mask */
-
-#define SCB_SHCSR_SVCALLACT_Pos             7                                             /*!< SCB SHCSR: SVCALLACT Position */
-#define SCB_SHCSR_SVCALLACT_Msk            (1UL << SCB_SHCSR_SVCALLACT_Pos)               /*!< SCB SHCSR: SVCALLACT Mask */
-
-#define SCB_SHCSR_USGFAULTACT_Pos           3                                             /*!< SCB SHCSR: USGFAULTACT Position */
-#define SCB_SHCSR_USGFAULTACT_Msk          (1UL << SCB_SHCSR_USGFAULTACT_Pos)             /*!< SCB SHCSR: USGFAULTACT Mask */
-
-#define SCB_SHCSR_BUSFAULTACT_Pos           1                                             /*!< SCB SHCSR: BUSFAULTACT Position */
-#define SCB_SHCSR_BUSFAULTACT_Msk          (1UL << SCB_SHCSR_BUSFAULTACT_Pos)             /*!< SCB SHCSR: BUSFAULTACT Mask */
-
-#define SCB_SHCSR_MEMFAULTACT_Pos           0                                             /*!< SCB SHCSR: MEMFAULTACT Position */
-#define SCB_SHCSR_MEMFAULTACT_Msk          (1UL << SCB_SHCSR_MEMFAULTACT_Pos)             /*!< SCB SHCSR: MEMFAULTACT Mask */
-
-/* SCB Configurable Fault Status Registers Definitions */
-#define SCB_CFSR_USGFAULTSR_Pos            16                                             /*!< SCB CFSR: Usage Fault Status Register Position */
-#define SCB_CFSR_USGFAULTSR_Msk            (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos)          /*!< SCB CFSR: Usage Fault Status Register Mask */
-
-#define SCB_CFSR_BUSFAULTSR_Pos             8                                             /*!< SCB CFSR: Bus Fault Status Register Position */
-#define SCB_CFSR_BUSFAULTSR_Msk            (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos)            /*!< SCB CFSR: Bus Fault Status Register Mask */
-
-#define SCB_CFSR_MEMFAULTSR_Pos             0                                             /*!< SCB CFSR: Memory Manage Fault Status Register Position */
-#define SCB_CFSR_MEMFAULTSR_Msk            (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos)            /*!< SCB CFSR: Memory Manage Fault Status Register Mask */
-
-/* SCB Hard Fault Status Registers Definitions */
-#define SCB_HFSR_DEBUGEVT_Pos              31                                             /*!< SCB HFSR: DEBUGEVT Position */
-#define SCB_HFSR_DEBUGEVT_Msk              (1UL << SCB_HFSR_DEBUGEVT_Pos)                 /*!< SCB HFSR: DEBUGEVT Mask */
-
-#define SCB_HFSR_FORCED_Pos                30                                             /*!< SCB HFSR: FORCED Position */
-#define SCB_HFSR_FORCED_Msk                (1UL << SCB_HFSR_FORCED_Pos)                   /*!< SCB HFSR: FORCED Mask */
-
-#define SCB_HFSR_VECTTBL_Pos                1                                             /*!< SCB HFSR: VECTTBL Position */
-#define SCB_HFSR_VECTTBL_Msk               (1UL << SCB_HFSR_VECTTBL_Pos)                  /*!< SCB HFSR: VECTTBL Mask */
-
-/* SCB Debug Fault Status Register Definitions */
-#define SCB_DFSR_EXTERNAL_Pos               4                                             /*!< SCB DFSR: EXTERNAL Position */
-#define SCB_DFSR_EXTERNAL_Msk              (1UL << SCB_DFSR_EXTERNAL_Pos)                 /*!< SCB DFSR: EXTERNAL Mask */
-
-#define SCB_DFSR_VCATCH_Pos                 3                                             /*!< SCB DFSR: VCATCH Position */
-#define SCB_DFSR_VCATCH_Msk                (1UL << SCB_DFSR_VCATCH_Pos)                   /*!< SCB DFSR: VCATCH Mask */
-
-#define SCB_DFSR_DWTTRAP_Pos                2                                             /*!< SCB DFSR: DWTTRAP Position */
-#define SCB_DFSR_DWTTRAP_Msk               (1UL << SCB_DFSR_DWTTRAP_Pos)                  /*!< SCB DFSR: DWTTRAP Mask */
-
-#define SCB_DFSR_BKPT_Pos                   1                                             /*!< SCB DFSR: BKPT Position */
-#define SCB_DFSR_BKPT_Msk                  (1UL << SCB_DFSR_BKPT_Pos)                     /*!< SCB DFSR: BKPT Mask */
-
-#define SCB_DFSR_HALTED_Pos                 0                                             /*!< SCB DFSR: HALTED Position */
-#define SCB_DFSR_HALTED_Msk                (1UL << SCB_DFSR_HALTED_Pos)                   /*!< SCB DFSR: HALTED Mask */
-
-/*@} end of group CMSIS_SCB */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)
-    \brief      Type definitions for the System Control and ID Register not in the SCB
-  @{
- */
-
-/** \brief  Structure type to access the System Control and ID Register not in the SCB.
- */
-typedef struct
-{
-       uint32_t RESERVED0[1];
-  __I  uint32_t ICTR;                    /*!< Offset: 0x004 (R/ )  Interrupt Controller Type Register      */
-#if ((defined __CM3_REV) && (__CM3_REV >= 0x200))
-  __IO uint32_t ACTLR;                   /*!< Offset: 0x008 (R/W)  Auxiliary Control Register      */
-#else
-       uint32_t RESERVED1[1];
-#endif
-} SCnSCB_Type;
-
-/* Interrupt Controller Type Register Definitions */
-#define SCnSCB_ICTR_INTLINESNUM_Pos         0                                          /*!< ICTR: INTLINESNUM Position */
-#define SCnSCB_ICTR_INTLINESNUM_Msk        (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos)      /*!< ICTR: INTLINESNUM Mask */
-
-/* Auxiliary Control Register Definitions */
-
-#define SCnSCB_ACTLR_DISFOLD_Pos            2                                          /*!< ACTLR: DISFOLD Position */
-#define SCnSCB_ACTLR_DISFOLD_Msk           (1UL << SCnSCB_ACTLR_DISFOLD_Pos)           /*!< ACTLR: DISFOLD Mask */
-
-#define SCnSCB_ACTLR_DISDEFWBUF_Pos         1                                          /*!< ACTLR: DISDEFWBUF Position */
-#define SCnSCB_ACTLR_DISDEFWBUF_Msk        (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos)        /*!< ACTLR: DISDEFWBUF Mask */
-
-#define SCnSCB_ACTLR_DISMCYCINT_Pos         0                                          /*!< ACTLR: DISMCYCINT Position */
-#define SCnSCB_ACTLR_DISMCYCINT_Msk        (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos)        /*!< ACTLR: DISMCYCINT Mask */
-
-/*@} end of group CMSIS_SCnotSCB */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_SysTick     System Tick Timer (SysTick)
-    \brief      Type definitions for the System Timer Registers.
-  @{
- */
-
-/** \brief  Structure type to access the System Timer (SysTick).
- */
-typedef struct
-{
-  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  SysTick Control and Status Register */
-  __IO uint32_t LOAD;                    /*!< Offset: 0x004 (R/W)  SysTick Reload Value Register       */
-  __IO uint32_t VAL;                     /*!< Offset: 0x008 (R/W)  SysTick Current Value Register      */
-  __I  uint32_t CALIB;                   /*!< Offset: 0x00C (R/ )  SysTick Calibration Register        */
-} SysTick_Type;
-
-/* SysTick Control / Status Register Definitions */
-#define SysTick_CTRL_COUNTFLAG_Pos         16                                             /*!< SysTick CTRL: COUNTFLAG Position */
-#define SysTick_CTRL_COUNTFLAG_Msk         (1UL << SysTick_CTRL_COUNTFLAG_Pos)            /*!< SysTick CTRL: COUNTFLAG Mask */
-
-#define SysTick_CTRL_CLKSOURCE_Pos          2                                             /*!< SysTick CTRL: CLKSOURCE Position */
-#define SysTick_CTRL_CLKSOURCE_Msk         (1UL << SysTick_CTRL_CLKSOURCE_Pos)            /*!< SysTick CTRL: CLKSOURCE Mask */
-
-#define SysTick_CTRL_TICKINT_Pos            1                                             /*!< SysTick CTRL: TICKINT Position */
-#define SysTick_CTRL_TICKINT_Msk           (1UL << SysTick_CTRL_TICKINT_Pos)              /*!< SysTick CTRL: TICKINT Mask */
-
-#define SysTick_CTRL_ENABLE_Pos             0                                             /*!< SysTick CTRL: ENABLE Position */
-#define SysTick_CTRL_ENABLE_Msk            (1UL << SysTick_CTRL_ENABLE_Pos)               /*!< SysTick CTRL: ENABLE Mask */
-
-/* SysTick Reload Register Definitions */
-#define SysTick_LOAD_RELOAD_Pos             0                                             /*!< SysTick LOAD: RELOAD Position */
-#define SysTick_LOAD_RELOAD_Msk            (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos)        /*!< SysTick LOAD: RELOAD Mask */
-
-/* SysTick Current Register Definitions */
-#define SysTick_VAL_CURRENT_Pos             0                                             /*!< SysTick VAL: CURRENT Position */
-#define SysTick_VAL_CURRENT_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick VAL: CURRENT Mask */
-
-/* SysTick Calibration Register Definitions */
-#define SysTick_CALIB_NOREF_Pos            31                                             /*!< SysTick CALIB: NOREF Position */
-#define SysTick_CALIB_NOREF_Msk            (1UL << SysTick_CALIB_NOREF_Pos)               /*!< SysTick CALIB: NOREF Mask */
-
-#define SysTick_CALIB_SKEW_Pos             30                                             /*!< SysTick CALIB: SKEW Position */
-#define SysTick_CALIB_SKEW_Msk             (1UL << SysTick_CALIB_SKEW_Pos)                /*!< SysTick CALIB: SKEW Mask */
-
-#define SysTick_CALIB_TENMS_Pos             0                                             /*!< SysTick CALIB: TENMS Position */
-#define SysTick_CALIB_TENMS_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick CALIB: TENMS Mask */
-
-/*@} end of group CMSIS_SysTick */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_ITM     Instrumentation Trace Macrocell (ITM)
-    \brief      Type definitions for the Instrumentation Trace Macrocell (ITM)
-  @{
- */
-
-/** \brief  Structure type to access the Instrumentation Trace Macrocell Register (ITM).
- */
-typedef struct
-{
-  __O  union
-  {
-    __O  uint8_t    u8;                  /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 8-bit                   */
-    __O  uint16_t   u16;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 16-bit                  */
-    __O  uint32_t   u32;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 32-bit                  */
-  }  PORT [32];                          /*!< Offset: 0x000 ( /W)  ITM Stimulus Port Registers               */
-       uint32_t RESERVED0[864];
-  __IO uint32_t TER;                     /*!< Offset: 0xE00 (R/W)  ITM Trace Enable Register                 */
-       uint32_t RESERVED1[15];
-  __IO uint32_t TPR;                     /*!< Offset: 0xE40 (R/W)  ITM Trace Privilege Register              */
-       uint32_t RESERVED2[15];
-  __IO uint32_t TCR;                     /*!< Offset: 0xE80 (R/W)  ITM Trace Control Register                */
-       uint32_t RESERVED3[29];
-  __O  uint32_t IWR;                     /*!< Offset: 0xEF8 ( /W)  ITM Integration Write Register            */
-  __I  uint32_t IRR;                     /*!< Offset: 0xEFC (R/ )  ITM Integration Read Register             */
-  __IO uint32_t IMCR;                    /*!< Offset: 0xF00 (R/W)  ITM Integration Mode Control Register     */
-       uint32_t RESERVED4[43];
-  __O  uint32_t LAR;                     /*!< Offset: 0xFB0 ( /W)  ITM Lock Access Register                  */
-  __I  uint32_t LSR;                     /*!< Offset: 0xFB4 (R/ )  ITM Lock Status Register                  */
-       uint32_t RESERVED5[6];
-  __I  uint32_t PID4;                    /*!< Offset: 0xFD0 (R/ )  ITM Peripheral Identification Register #4 */
-  __I  uint32_t PID5;                    /*!< Offset: 0xFD4 (R/ )  ITM Peripheral Identification Register #5 */
-  __I  uint32_t PID6;                    /*!< Offset: 0xFD8 (R/ )  ITM Peripheral Identification Register #6 */
-  __I  uint32_t PID7;                    /*!< Offset: 0xFDC (R/ )  ITM Peripheral Identification Register #7 */
-  __I  uint32_t PID0;                    /*!< Offset: 0xFE0 (R/ )  ITM Peripheral Identification Register #0 */
-  __I  uint32_t PID1;                    /*!< Offset: 0xFE4 (R/ )  ITM Peripheral Identification Register #1 */
-  __I  uint32_t PID2;                    /*!< Offset: 0xFE8 (R/ )  ITM Peripheral Identification Register #2 */
-  __I  uint32_t PID3;                    /*!< Offset: 0xFEC (R/ )  ITM Peripheral Identification Register #3 */
-  __I  uint32_t CID0;                    /*!< Offset: 0xFF0 (R/ )  ITM Component  Identification Register #0 */
-  __I  uint32_t CID1;                    /*!< Offset: 0xFF4 (R/ )  ITM Component  Identification Register #1 */
-  __I  uint32_t CID2;                    /*!< Offset: 0xFF8 (R/ )  ITM Component  Identification Register #2 */
-  __I  uint32_t CID3;                    /*!< Offset: 0xFFC (R/ )  ITM Component  Identification Register #3 */
-} ITM_Type;
-
-/* ITM Trace Privilege Register Definitions */
-#define ITM_TPR_PRIVMASK_Pos                0                                             /*!< ITM TPR: PRIVMASK Position */
-#define ITM_TPR_PRIVMASK_Msk               (0xFUL << ITM_TPR_PRIVMASK_Pos)                /*!< ITM TPR: PRIVMASK Mask */
-
-/* ITM Trace Control Register Definitions */
-#define ITM_TCR_BUSY_Pos                   23                                             /*!< ITM TCR: BUSY Position */
-#define ITM_TCR_BUSY_Msk                   (1UL << ITM_TCR_BUSY_Pos)                      /*!< ITM TCR: BUSY Mask */
-
-#define ITM_TCR_TraceBusID_Pos             16                                             /*!< ITM TCR: ATBID Position */
-#define ITM_TCR_TraceBusID_Msk             (0x7FUL << ITM_TCR_TraceBusID_Pos)             /*!< ITM TCR: ATBID Mask */
-
-#define ITM_TCR_GTSFREQ_Pos                10                                             /*!< ITM TCR: Global timestamp frequency Position */
-#define ITM_TCR_GTSFREQ_Msk                (3UL << ITM_TCR_GTSFREQ_Pos)                   /*!< ITM TCR: Global timestamp frequency Mask */
-
-#define ITM_TCR_TSPrescale_Pos              8                                             /*!< ITM TCR: TSPrescale Position */
-#define ITM_TCR_TSPrescale_Msk             (3UL << ITM_TCR_TSPrescale_Pos)                /*!< ITM TCR: TSPrescale Mask */
-
-#define ITM_TCR_SWOENA_Pos                  4                                             /*!< ITM TCR: SWOENA Position */
-#define ITM_TCR_SWOENA_Msk                 (1UL << ITM_TCR_SWOENA_Pos)                    /*!< ITM TCR: SWOENA Mask */
-
-#define ITM_TCR_DWTENA_Pos                  3                                             /*!< ITM TCR: DWTENA Position */
-#define ITM_TCR_DWTENA_Msk                 (1UL << ITM_TCR_DWTENA_Pos)                    /*!< ITM TCR: DWTENA Mask */
-
-#define ITM_TCR_SYNCENA_Pos                 2                                             /*!< ITM TCR: SYNCENA Position */
-#define ITM_TCR_SYNCENA_Msk                (1UL << ITM_TCR_SYNCENA_Pos)                   /*!< ITM TCR: SYNCENA Mask */
-
-#define ITM_TCR_TSENA_Pos                   1                                             /*!< ITM TCR: TSENA Position */
-#define ITM_TCR_TSENA_Msk                  (1UL << ITM_TCR_TSENA_Pos)                     /*!< ITM TCR: TSENA Mask */
-
-#define ITM_TCR_ITMENA_Pos                  0                                             /*!< ITM TCR: ITM Enable bit Position */
-#define ITM_TCR_ITMENA_Msk                 (1UL << ITM_TCR_ITMENA_Pos)                    /*!< ITM TCR: ITM Enable bit Mask */
-
-/* ITM Integration Write Register Definitions */
-#define ITM_IWR_ATVALIDM_Pos                0                                             /*!< ITM IWR: ATVALIDM Position */
-#define ITM_IWR_ATVALIDM_Msk               (1UL << ITM_IWR_ATVALIDM_Pos)                  /*!< ITM IWR: ATVALIDM Mask */
-
-/* ITM Integration Read Register Definitions */
-#define ITM_IRR_ATREADYM_Pos                0                                             /*!< ITM IRR: ATREADYM Position */
-#define ITM_IRR_ATREADYM_Msk               (1UL << ITM_IRR_ATREADYM_Pos)                  /*!< ITM IRR: ATREADYM Mask */
-
-/* ITM Integration Mode Control Register Definitions */
-#define ITM_IMCR_INTEGRATION_Pos            0                                             /*!< ITM IMCR: INTEGRATION Position */
-#define ITM_IMCR_INTEGRATION_Msk           (1UL << ITM_IMCR_INTEGRATION_Pos)              /*!< ITM IMCR: INTEGRATION Mask */
-
-/* ITM Lock Status Register Definitions */
-#define ITM_LSR_ByteAcc_Pos                 2                                             /*!< ITM LSR: ByteAcc Position */
-#define ITM_LSR_ByteAcc_Msk                (1UL << ITM_LSR_ByteAcc_Pos)                   /*!< ITM LSR: ByteAcc Mask */
-
-#define ITM_LSR_Access_Pos                  1                                             /*!< ITM LSR: Access Position */
-#define ITM_LSR_Access_Msk                 (1UL << ITM_LSR_Access_Pos)                    /*!< ITM LSR: Access Mask */
-
-#define ITM_LSR_Present_Pos                 0                                             /*!< ITM LSR: Present Position */
-#define ITM_LSR_Present_Msk                (1UL << ITM_LSR_Present_Pos)                   /*!< ITM LSR: Present Mask */
-
-/*@}*/ /* end of group CMSIS_ITM */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_DWT     Data Watchpoint and Trace (DWT)
-    \brief      Type definitions for the Data Watchpoint and Trace (DWT)
-  @{
- */
-
-/** \brief  Structure type to access the Data Watchpoint and Trace Register (DWT).
- */
-typedef struct
-{
-  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  Control Register                          */
-  __IO uint32_t CYCCNT;                  /*!< Offset: 0x004 (R/W)  Cycle Count Register                      */
-  __IO uint32_t CPICNT;                  /*!< Offset: 0x008 (R/W)  CPI Count Register                        */
-  __IO uint32_t EXCCNT;                  /*!< Offset: 0x00C (R/W)  Exception Overhead Count Register         */
-  __IO uint32_t SLEEPCNT;                /*!< Offset: 0x010 (R/W)  Sleep Count Register                      */
-  __IO uint32_t LSUCNT;                  /*!< Offset: 0x014 (R/W)  LSU Count Register                        */
-  __IO uint32_t FOLDCNT;                 /*!< Offset: 0x018 (R/W)  Folded-instruction Count Register         */
-  __I  uint32_t PCSR;                    /*!< Offset: 0x01C (R/ )  Program Counter Sample Register           */
-  __IO uint32_t COMP0;                   /*!< Offset: 0x020 (R/W)  Comparator Register 0                     */
-  __IO uint32_t MASK0;                   /*!< Offset: 0x024 (R/W)  Mask Register 0                           */
-  __IO uint32_t FUNCTION0;               /*!< Offset: 0x028 (R/W)  Function Register 0                       */
-       uint32_t RESERVED0[1];
-  __IO uint32_t COMP1;                   /*!< Offset: 0x030 (R/W)  Comparator Register 1                     */
-  __IO uint32_t MASK1;                   /*!< Offset: 0x034 (R/W)  Mask Register 1                           */
-  __IO uint32_t FUNCTION1;               /*!< Offset: 0x038 (R/W)  Function Register 1                       */
-       uint32_t RESERVED1[1];
-  __IO uint32_t COMP2;                   /*!< Offset: 0x040 (R/W)  Comparator Register 2                     */
-  __IO uint32_t MASK2;                   /*!< Offset: 0x044 (R/W)  Mask Register 2                           */
-  __IO uint32_t FUNCTION2;               /*!< Offset: 0x048 (R/W)  Function Register 2                       */
-       uint32_t RESERVED2[1];
-  __IO uint32_t COMP3;                   /*!< Offset: 0x050 (R/W)  Comparator Register 3                     */
-  __IO uint32_t MASK3;                   /*!< Offset: 0x054 (R/W)  Mask Register 3                           */
-  __IO uint32_t FUNCTION3;               /*!< Offset: 0x058 (R/W)  Function Register 3                       */
-} DWT_Type;
-
-/* DWT Control Register Definitions */
-#define DWT_CTRL_NUMCOMP_Pos               28                                          /*!< DWT CTRL: NUMCOMP Position */
-#define DWT_CTRL_NUMCOMP_Msk               (0xFUL << DWT_CTRL_NUMCOMP_Pos)             /*!< DWT CTRL: NUMCOMP Mask */
-
-#define DWT_CTRL_NOTRCPKT_Pos              27                                          /*!< DWT CTRL: NOTRCPKT Position */
-#define DWT_CTRL_NOTRCPKT_Msk              (0x1UL << DWT_CTRL_NOTRCPKT_Pos)            /*!< DWT CTRL: NOTRCPKT Mask */
-
-#define DWT_CTRL_NOEXTTRIG_Pos             26                                          /*!< DWT CTRL: NOEXTTRIG Position */
-#define DWT_CTRL_NOEXTTRIG_Msk             (0x1UL << DWT_CTRL_NOEXTTRIG_Pos)           /*!< DWT CTRL: NOEXTTRIG Mask */
-
-#define DWT_CTRL_NOCYCCNT_Pos              25                                          /*!< DWT CTRL: NOCYCCNT Position */
-#define DWT_CTRL_NOCYCCNT_Msk              (0x1UL << DWT_CTRL_NOCYCCNT_Pos)            /*!< DWT CTRL: NOCYCCNT Mask */
-
-#define DWT_CTRL_NOPRFCNT_Pos              24                                          /*!< DWT CTRL: NOPRFCNT Position */
-#define DWT_CTRL_NOPRFCNT_Msk              (0x1UL << DWT_CTRL_NOPRFCNT_Pos)            /*!< DWT CTRL: NOPRFCNT Mask */
-
-#define DWT_CTRL_CYCEVTENA_Pos             22                                          /*!< DWT CTRL: CYCEVTENA Position */
-#define DWT_CTRL_CYCEVTENA_Msk             (0x1UL << DWT_CTRL_CYCEVTENA_Pos)           /*!< DWT CTRL: CYCEVTENA Mask */
-
-#define DWT_CTRL_FOLDEVTENA_Pos            21                                          /*!< DWT CTRL: FOLDEVTENA Position */
-#define DWT_CTRL_FOLDEVTENA_Msk            (0x1UL << DWT_CTRL_FOLDEVTENA_Pos)          /*!< DWT CTRL: FOLDEVTENA Mask */
-
-#define DWT_CTRL_LSUEVTENA_Pos             20                                          /*!< DWT CTRL: LSUEVTENA Position */
-#define DWT_CTRL_LSUEVTENA_Msk             (0x1UL << DWT_CTRL_LSUEVTENA_Pos)           /*!< DWT CTRL: LSUEVTENA Mask */
-
-#define DWT_CTRL_SLEEPEVTENA_Pos           19                                          /*!< DWT CTRL: SLEEPEVTENA Position */
-#define DWT_CTRL_SLEEPEVTENA_Msk           (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos)         /*!< DWT CTRL: SLEEPEVTENA Mask */
-
-#define DWT_CTRL_EXCEVTENA_Pos             18                                          /*!< DWT CTRL: EXCEVTENA Position */
-#define DWT_CTRL_EXCEVTENA_Msk             (0x1UL << DWT_CTRL_EXCEVTENA_Pos)           /*!< DWT CTRL: EXCEVTENA Mask */
-
-#define DWT_CTRL_CPIEVTENA_Pos             17                                          /*!< DWT CTRL: CPIEVTENA Position */
-#define DWT_CTRL_CPIEVTENA_Msk             (0x1UL << DWT_CTRL_CPIEVTENA_Pos)           /*!< DWT CTRL: CPIEVTENA Mask */
-
-#define DWT_CTRL_EXCTRCENA_Pos             16                                          /*!< DWT CTRL: EXCTRCENA Position */
-#define DWT_CTRL_EXCTRCENA_Msk             (0x1UL << DWT_CTRL_EXCTRCENA_Pos)           /*!< DWT CTRL: EXCTRCENA Mask */
-
-#define DWT_CTRL_PCSAMPLENA_Pos            12                                          /*!< DWT CTRL: PCSAMPLENA Position */
-#define DWT_CTRL_PCSAMPLENA_Msk            (0x1UL << DWT_CTRL_PCSAMPLENA_Pos)          /*!< DWT CTRL: PCSAMPLENA Mask */
-
-#define DWT_CTRL_SYNCTAP_Pos               10                                          /*!< DWT CTRL: SYNCTAP Position */
-#define DWT_CTRL_SYNCTAP_Msk               (0x3UL << DWT_CTRL_SYNCTAP_Pos)             /*!< DWT CTRL: SYNCTAP Mask */
-
-#define DWT_CTRL_CYCTAP_Pos                 9                                          /*!< DWT CTRL: CYCTAP Position */
-#define DWT_CTRL_CYCTAP_Msk                (0x1UL << DWT_CTRL_CYCTAP_Pos)              /*!< DWT CTRL: CYCTAP Mask */
-
-#define DWT_CTRL_POSTINIT_Pos               5                                          /*!< DWT CTRL: POSTINIT Position */
-#define DWT_CTRL_POSTINIT_Msk              (0xFUL << DWT_CTRL_POSTINIT_Pos)            /*!< DWT CTRL: POSTINIT Mask */
-
-#define DWT_CTRL_POSTPRESET_Pos             1                                          /*!< DWT CTRL: POSTPRESET Position */
-#define DWT_CTRL_POSTPRESET_Msk            (0xFUL << DWT_CTRL_POSTPRESET_Pos)          /*!< DWT CTRL: POSTPRESET Mask */
-
-#define DWT_CTRL_CYCCNTENA_Pos              0                                          /*!< DWT CTRL: CYCCNTENA Position */
-#define DWT_CTRL_CYCCNTENA_Msk             (0x1UL << DWT_CTRL_CYCCNTENA_Pos)           /*!< DWT CTRL: CYCCNTENA Mask */
-
-/* DWT CPI Count Register Definitions */
-#define DWT_CPICNT_CPICNT_Pos               0                                          /*!< DWT CPICNT: CPICNT Position */
-#define DWT_CPICNT_CPICNT_Msk              (0xFFUL << DWT_CPICNT_CPICNT_Pos)           /*!< DWT CPICNT: CPICNT Mask */
-
-/* DWT Exception Overhead Count Register Definitions */
-#define DWT_EXCCNT_EXCCNT_Pos               0                                          /*!< DWT EXCCNT: EXCCNT Position */
-#define DWT_EXCCNT_EXCCNT_Msk              (0xFFUL << DWT_EXCCNT_EXCCNT_Pos)           /*!< DWT EXCCNT: EXCCNT Mask */
-
-/* DWT Sleep Count Register Definitions */
-#define DWT_SLEEPCNT_SLEEPCNT_Pos           0                                          /*!< DWT SLEEPCNT: SLEEPCNT Position */
-#define DWT_SLEEPCNT_SLEEPCNT_Msk          (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos)       /*!< DWT SLEEPCNT: SLEEPCNT Mask */
-
-/* DWT LSU Count Register Definitions */
-#define DWT_LSUCNT_LSUCNT_Pos               0                                          /*!< DWT LSUCNT: LSUCNT Position */
-#define DWT_LSUCNT_LSUCNT_Msk              (0xFFUL << DWT_LSUCNT_LSUCNT_Pos)           /*!< DWT LSUCNT: LSUCNT Mask */
-
-/* DWT Folded-instruction Count Register Definitions */
-#define DWT_FOLDCNT_FOLDCNT_Pos             0                                          /*!< DWT FOLDCNT: FOLDCNT Position */
-#define DWT_FOLDCNT_FOLDCNT_Msk            (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos)         /*!< DWT FOLDCNT: FOLDCNT Mask */
-
-/* DWT Comparator Mask Register Definitions */
-#define DWT_MASK_MASK_Pos                   0                                          /*!< DWT MASK: MASK Position */
-#define DWT_MASK_MASK_Msk                  (0x1FUL << DWT_MASK_MASK_Pos)               /*!< DWT MASK: MASK Mask */
-
-/* DWT Comparator Function Register Definitions */
-#define DWT_FUNCTION_MATCHED_Pos           24                                          /*!< DWT FUNCTION: MATCHED Position */
-#define DWT_FUNCTION_MATCHED_Msk           (0x1UL << DWT_FUNCTION_MATCHED_Pos)         /*!< DWT FUNCTION: MATCHED Mask */
-
-#define DWT_FUNCTION_DATAVADDR1_Pos        16                                          /*!< DWT FUNCTION: DATAVADDR1 Position */
-#define DWT_FUNCTION_DATAVADDR1_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos)      /*!< DWT FUNCTION: DATAVADDR1 Mask */
-
-#define DWT_FUNCTION_DATAVADDR0_Pos        12                                          /*!< DWT FUNCTION: DATAVADDR0 Position */
-#define DWT_FUNCTION_DATAVADDR0_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos)      /*!< DWT FUNCTION: DATAVADDR0 Mask */
-
-#define DWT_FUNCTION_DATAVSIZE_Pos         10                                          /*!< DWT FUNCTION: DATAVSIZE Position */
-#define DWT_FUNCTION_DATAVSIZE_Msk         (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos)       /*!< DWT FUNCTION: DATAVSIZE Mask */
-
-#define DWT_FUNCTION_LNK1ENA_Pos            9                                          /*!< DWT FUNCTION: LNK1ENA Position */
-#define DWT_FUNCTION_LNK1ENA_Msk           (0x1UL << DWT_FUNCTION_LNK1ENA_Pos)         /*!< DWT FUNCTION: LNK1ENA Mask */
-
-#define DWT_FUNCTION_DATAVMATCH_Pos         8                                          /*!< DWT FUNCTION: DATAVMATCH Position */
-#define DWT_FUNCTION_DATAVMATCH_Msk        (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos)      /*!< DWT FUNCTION: DATAVMATCH Mask */
-
-#define DWT_FUNCTION_CYCMATCH_Pos           7                                          /*!< DWT FUNCTION: CYCMATCH Position */
-#define DWT_FUNCTION_CYCMATCH_Msk          (0x1UL << DWT_FUNCTION_CYCMATCH_Pos)        /*!< DWT FUNCTION: CYCMATCH Mask */
-
-#define DWT_FUNCTION_EMITRANGE_Pos          5                                          /*!< DWT FUNCTION: EMITRANGE Position */
-#define DWT_FUNCTION_EMITRANGE_Msk         (0x1UL << DWT_FUNCTION_EMITRANGE_Pos)       /*!< DWT FUNCTION: EMITRANGE Mask */
-
-#define DWT_FUNCTION_FUNCTION_Pos           0                                          /*!< DWT FUNCTION: FUNCTION Position */
-#define DWT_FUNCTION_FUNCTION_Msk          (0xFUL << DWT_FUNCTION_FUNCTION_Pos)        /*!< DWT FUNCTION: FUNCTION Mask */
-
-/*@}*/ /* end of group CMSIS_DWT */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_TPI     Trace Port Interface (TPI)
-    \brief      Type definitions for the Trace Port Interface (TPI)
-  @{
- */
-
-/** \brief  Structure type to access the Trace Port Interface Register (TPI).
- */
-typedef struct
-{
-  __IO uint32_t SSPSR;                   /*!< Offset: 0x000 (R/ )  Supported Parallel Port Size Register     */
-  __IO uint32_t CSPSR;                   /*!< Offset: 0x004 (R/W)  Current Parallel Port Size Register */
-       uint32_t RESERVED0[2];
-  __IO uint32_t ACPR;                    /*!< Offset: 0x010 (R/W)  Asynchronous Clock Prescaler Register */
-       uint32_t RESERVED1[55];
-  __IO uint32_t SPPR;                    /*!< Offset: 0x0F0 (R/W)  Selected Pin Protocol Register */
-       uint32_t RESERVED2[131];
-  __I  uint32_t FFSR;                    /*!< Offset: 0x300 (R/ )  Formatter and Flush Status Register */
-  __IO uint32_t FFCR;                    /*!< Offset: 0x304 (R/W)  Formatter and Flush Control Register */
-  __I  uint32_t FSCR;                    /*!< Offset: 0x308 (R/ )  Formatter Synchronization Counter Register */
-       uint32_t RESERVED3[759];
-  __I  uint32_t TRIGGER;                 /*!< Offset: 0xEE8 (R/ )  TRIGGER */
-  __I  uint32_t FIFO0;                   /*!< Offset: 0xEEC (R/ )  Integration ETM Data */
-  __I  uint32_t ITATBCTR2;               /*!< Offset: 0xEF0 (R/ )  ITATBCTR2 */
-       uint32_t RESERVED4[1];
-  __I  uint32_t ITATBCTR0;               /*!< Offset: 0xEF8 (R/ )  ITATBCTR0 */
-  __I  uint32_t FIFO1;                   /*!< Offset: 0xEFC (R/ )  Integration ITM Data */
-  __IO uint32_t ITCTRL;                  /*!< Offset: 0xF00 (R/W)  Integration Mode Control */
-       uint32_t RESERVED5[39];
-  __IO uint32_t CLAIMSET;                /*!< Offset: 0xFA0 (R/W)  Claim tag set */
-  __IO uint32_t CLAIMCLR;                /*!< Offset: 0xFA4 (R/W)  Claim tag clear */
-       uint32_t RESERVED7[8];
-  __I  uint32_t DEVID;                   /*!< Offset: 0xFC8 (R/ )  TPIU_DEVID */
-  __I  uint32_t DEVTYPE;                 /*!< Offset: 0xFCC (R/ )  TPIU_DEVTYPE */
-} TPI_Type;
-
-/* TPI Asynchronous Clock Prescaler Register Definitions */
-#define TPI_ACPR_PRESCALER_Pos              0                                          /*!< TPI ACPR: PRESCALER Position */
-#define TPI_ACPR_PRESCALER_Msk             (0x1FFFUL << TPI_ACPR_PRESCALER_Pos)        /*!< TPI ACPR: PRESCALER Mask */
-
-/* TPI Selected Pin Protocol Register Definitions */
-#define TPI_SPPR_TXMODE_Pos                 0                                          /*!< TPI SPPR: TXMODE Position */
-#define TPI_SPPR_TXMODE_Msk                (0x3UL << TPI_SPPR_TXMODE_Pos)              /*!< TPI SPPR: TXMODE Mask */
-
-/* TPI Formatter and Flush Status Register Definitions */
-#define TPI_FFSR_FtNonStop_Pos              3                                          /*!< TPI FFSR: FtNonStop Position */
-#define TPI_FFSR_FtNonStop_Msk             (0x1UL << TPI_FFSR_FtNonStop_Pos)           /*!< TPI FFSR: FtNonStop Mask */
-
-#define TPI_FFSR_TCPresent_Pos              2                                          /*!< TPI FFSR: TCPresent Position */
-#define TPI_FFSR_TCPresent_Msk             (0x1UL << TPI_FFSR_TCPresent_Pos)           /*!< TPI FFSR: TCPresent Mask */
-
-#define TPI_FFSR_FtStopped_Pos              1                                          /*!< TPI FFSR: FtStopped Position */
-#define TPI_FFSR_FtStopped_Msk             (0x1UL << TPI_FFSR_FtStopped_Pos)           /*!< TPI FFSR: FtStopped Mask */
-
-#define TPI_FFSR_FlInProg_Pos               0                                          /*!< TPI FFSR: FlInProg Position */
-#define TPI_FFSR_FlInProg_Msk              (0x1UL << TPI_FFSR_FlInProg_Pos)            /*!< TPI FFSR: FlInProg Mask */
-
-/* TPI Formatter and Flush Control Register Definitions */
-#define TPI_FFCR_TrigIn_Pos                 8                                          /*!< TPI FFCR: TrigIn Position */
-#define TPI_FFCR_TrigIn_Msk                (0x1UL << TPI_FFCR_TrigIn_Pos)              /*!< TPI FFCR: TrigIn Mask */
-
-#define TPI_FFCR_EnFCont_Pos                1                                          /*!< TPI FFCR: EnFCont Position */
-#define TPI_FFCR_EnFCont_Msk               (0x1UL << TPI_FFCR_EnFCont_Pos)             /*!< TPI FFCR: EnFCont Mask */
-
-/* TPI TRIGGER Register Definitions */
-#define TPI_TRIGGER_TRIGGER_Pos             0                                          /*!< TPI TRIGGER: TRIGGER Position */
-#define TPI_TRIGGER_TRIGGER_Msk            (0x1UL << TPI_TRIGGER_TRIGGER_Pos)          /*!< TPI TRIGGER: TRIGGER Mask */
-
-/* TPI Integration ETM Data Register Definitions (FIFO0) */
-#define TPI_FIFO0_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO0: ITM_ATVALID Position */
-#define TPI_FIFO0_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos)        /*!< TPI FIFO0: ITM_ATVALID Mask */
-
-#define TPI_FIFO0_ITM_bytecount_Pos        27                                          /*!< TPI FIFO0: ITM_bytecount Position */
-#define TPI_FIFO0_ITM_bytecount_Msk        (0x3UL << TPI_FIFO0_ITM_bytecount_Pos)      /*!< TPI FIFO0: ITM_bytecount Mask */
-
-#define TPI_FIFO0_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO0: ETM_ATVALID Position */
-#define TPI_FIFO0_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos)        /*!< TPI FIFO0: ETM_ATVALID Mask */
-
-#define TPI_FIFO0_ETM_bytecount_Pos        24                                          /*!< TPI FIFO0: ETM_bytecount Position */
-#define TPI_FIFO0_ETM_bytecount_Msk        (0x3UL << TPI_FIFO0_ETM_bytecount_Pos)      /*!< TPI FIFO0: ETM_bytecount Mask */
-
-#define TPI_FIFO0_ETM2_Pos                 16                                          /*!< TPI FIFO0: ETM2 Position */
-#define TPI_FIFO0_ETM2_Msk                 (0xFFUL << TPI_FIFO0_ETM2_Pos)              /*!< TPI FIFO0: ETM2 Mask */
-
-#define TPI_FIFO0_ETM1_Pos                  8                                          /*!< TPI FIFO0: ETM1 Position */
-#define TPI_FIFO0_ETM1_Msk                 (0xFFUL << TPI_FIFO0_ETM1_Pos)              /*!< TPI FIFO0: ETM1 Mask */
-
-#define TPI_FIFO0_ETM0_Pos                  0                                          /*!< TPI FIFO0: ETM0 Position */
-#define TPI_FIFO0_ETM0_Msk                 (0xFFUL << TPI_FIFO0_ETM0_Pos)              /*!< TPI FIFO0: ETM0 Mask */
-
-/* TPI ITATBCTR2 Register Definitions */
-#define TPI_ITATBCTR2_ATREADY_Pos           0                                          /*!< TPI ITATBCTR2: ATREADY Position */
-#define TPI_ITATBCTR2_ATREADY_Msk          (0x1UL << TPI_ITATBCTR2_ATREADY_Pos)        /*!< TPI ITATBCTR2: ATREADY Mask */
-
-/* TPI Integration ITM Data Register Definitions (FIFO1) */
-#define TPI_FIFO1_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO1: ITM_ATVALID Position */
-#define TPI_FIFO1_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos)        /*!< TPI FIFO1: ITM_ATVALID Mask */
-
-#define TPI_FIFO1_ITM_bytecount_Pos        27                                          /*!< TPI FIFO1: ITM_bytecount Position */
-#define TPI_FIFO1_ITM_bytecount_Msk        (0x3UL << TPI_FIFO1_ITM_bytecount_Pos)      /*!< TPI FIFO1: ITM_bytecount Mask */
-
-#define TPI_FIFO1_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO1: ETM_ATVALID Position */
-#define TPI_FIFO1_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos)        /*!< TPI FIFO1: ETM_ATVALID Mask */
-
-#define TPI_FIFO1_ETM_bytecount_Pos        24                                          /*!< TPI FIFO1: ETM_bytecount Position */
-#define TPI_FIFO1_ETM_bytecount_Msk        (0x3UL << TPI_FIFO1_ETM_bytecount_Pos)      /*!< TPI FIFO1: ETM_bytecount Mask */
-
-#define TPI_FIFO1_ITM2_Pos                 16                                          /*!< TPI FIFO1: ITM2 Position */
-#define TPI_FIFO1_ITM2_Msk                 (0xFFUL << TPI_FIFO1_ITM2_Pos)              /*!< TPI FIFO1: ITM2 Mask */
-
-#define TPI_FIFO1_ITM1_Pos                  8                                          /*!< TPI FIFO1: ITM1 Position */
-#define TPI_FIFO1_ITM1_Msk                 (0xFFUL << TPI_FIFO1_ITM1_Pos)              /*!< TPI FIFO1: ITM1 Mask */
-
-#define TPI_FIFO1_ITM0_Pos                  0                                          /*!< TPI FIFO1: ITM0 Position */
-#define TPI_FIFO1_ITM0_Msk                 (0xFFUL << TPI_FIFO1_ITM0_Pos)              /*!< TPI FIFO1: ITM0 Mask */
-
-/* TPI ITATBCTR0 Register Definitions */
-#define TPI_ITATBCTR0_ATREADY_Pos           0                                          /*!< TPI ITATBCTR0: ATREADY Position */
-#define TPI_ITATBCTR0_ATREADY_Msk          (0x1UL << TPI_ITATBCTR0_ATREADY_Pos)        /*!< TPI ITATBCTR0: ATREADY Mask */
-
-/* TPI Integration Mode Control Register Definitions */
-#define TPI_ITCTRL_Mode_Pos                 0                                          /*!< TPI ITCTRL: Mode Position */
-#define TPI_ITCTRL_Mode_Msk                (0x1UL << TPI_ITCTRL_Mode_Pos)              /*!< TPI ITCTRL: Mode Mask */
-
-/* TPI DEVID Register Definitions */
-#define TPI_DEVID_NRZVALID_Pos             11                                          /*!< TPI DEVID: NRZVALID Position */
-#define TPI_DEVID_NRZVALID_Msk             (0x1UL << TPI_DEVID_NRZVALID_Pos)           /*!< TPI DEVID: NRZVALID Mask */
-
-#define TPI_DEVID_MANCVALID_Pos            10                                          /*!< TPI DEVID: MANCVALID Position */
-#define TPI_DEVID_MANCVALID_Msk            (0x1UL << TPI_DEVID_MANCVALID_Pos)          /*!< TPI DEVID: MANCVALID Mask */
-
-#define TPI_DEVID_PTINVALID_Pos             9                                          /*!< TPI DEVID: PTINVALID Position */
-#define TPI_DEVID_PTINVALID_Msk            (0x1UL << TPI_DEVID_PTINVALID_Pos)          /*!< TPI DEVID: PTINVALID Mask */
-
-#define TPI_DEVID_MinBufSz_Pos              6                                          /*!< TPI DEVID: MinBufSz Position */
-#define TPI_DEVID_MinBufSz_Msk             (0x7UL << TPI_DEVID_MinBufSz_Pos)           /*!< TPI DEVID: MinBufSz Mask */
-
-#define TPI_DEVID_AsynClkIn_Pos             5                                          /*!< TPI DEVID: AsynClkIn Position */
-#define TPI_DEVID_AsynClkIn_Msk            (0x1UL << TPI_DEVID_AsynClkIn_Pos)          /*!< TPI DEVID: AsynClkIn Mask */
-
-#define TPI_DEVID_NrTraceInput_Pos          0                                          /*!< TPI DEVID: NrTraceInput Position */
-#define TPI_DEVID_NrTraceInput_Msk         (0x1FUL << TPI_DEVID_NrTraceInput_Pos)      /*!< TPI DEVID: NrTraceInput Mask */
-
-/* TPI DEVTYPE Register Definitions */
-#define TPI_DEVTYPE_SubType_Pos             0                                          /*!< TPI DEVTYPE: SubType Position */
-#define TPI_DEVTYPE_SubType_Msk            (0xFUL << TPI_DEVTYPE_SubType_Pos)          /*!< TPI DEVTYPE: SubType Mask */
-
-#define TPI_DEVTYPE_MajorType_Pos           4                                          /*!< TPI DEVTYPE: MajorType Position */
-#define TPI_DEVTYPE_MajorType_Msk          (0xFUL << TPI_DEVTYPE_MajorType_Pos)        /*!< TPI DEVTYPE: MajorType Mask */
-
-/*@}*/ /* end of group CMSIS_TPI */
-
-
-#if (__MPU_PRESENT == 1)
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_MPU     Memory Protection Unit (MPU)
-    \brief      Type definitions for the Memory Protection Unit (MPU)
-  @{
- */
-
-/** \brief  Structure type to access the Memory Protection Unit (MPU).
- */
-typedef struct
-{
-  __I  uint32_t TYPE;                    /*!< Offset: 0x000 (R/ )  MPU Type Register                              */
-  __IO uint32_t CTRL;                    /*!< Offset: 0x004 (R/W)  MPU Control Register                           */
-  __IO uint32_t RNR;                     /*!< Offset: 0x008 (R/W)  MPU Region RNRber Register                     */
-  __IO uint32_t RBAR;                    /*!< Offset: 0x00C (R/W)  MPU Region Base Address Register               */
-  __IO uint32_t RASR;                    /*!< Offset: 0x010 (R/W)  MPU Region Attribute and Size Register         */
-  __IO uint32_t RBAR_A1;                 /*!< Offset: 0x014 (R/W)  MPU Alias 1 Region Base Address Register       */
-  __IO uint32_t RASR_A1;                 /*!< Offset: 0x018 (R/W)  MPU Alias 1 Region Attribute and Size Register */
-  __IO uint32_t RBAR_A2;                 /*!< Offset: 0x01C (R/W)  MPU Alias 2 Region Base Address Register       */
-  __IO uint32_t RASR_A2;                 /*!< Offset: 0x020 (R/W)  MPU Alias 2 Region Attribute and Size Register */
-  __IO uint32_t RBAR_A3;                 /*!< Offset: 0x024 (R/W)  MPU Alias 3 Region Base Address Register       */
-  __IO uint32_t RASR_A3;                 /*!< Offset: 0x028 (R/W)  MPU Alias 3 Region Attribute and Size Register */
-} MPU_Type;
-
-/* MPU Type Register */
-#define MPU_TYPE_IREGION_Pos               16                                             /*!< MPU TYPE: IREGION Position */
-#define MPU_TYPE_IREGION_Msk               (0xFFUL << MPU_TYPE_IREGION_Pos)               /*!< MPU TYPE: IREGION Mask */
-
-#define MPU_TYPE_DREGION_Pos                8                                             /*!< MPU TYPE: DREGION Position */
-#define MPU_TYPE_DREGION_Msk               (0xFFUL << MPU_TYPE_DREGION_Pos)               /*!< MPU TYPE: DREGION Mask */
-
-#define MPU_TYPE_SEPARATE_Pos               0                                             /*!< MPU TYPE: SEPARATE Position */
-#define MPU_TYPE_SEPARATE_Msk              (1UL << MPU_TYPE_SEPARATE_Pos)                 /*!< MPU TYPE: SEPARATE Mask */
-
-/* MPU Control Register */
-#define MPU_CTRL_PRIVDEFENA_Pos             2                                             /*!< MPU CTRL: PRIVDEFENA Position */
-#define MPU_CTRL_PRIVDEFENA_Msk            (1UL << MPU_CTRL_PRIVDEFENA_Pos)               /*!< MPU CTRL: PRIVDEFENA Mask */
-
-#define MPU_CTRL_HFNMIENA_Pos               1                                             /*!< MPU CTRL: HFNMIENA Position */
-#define MPU_CTRL_HFNMIENA_Msk              (1UL << MPU_CTRL_HFNMIENA_Pos)                 /*!< MPU CTRL: HFNMIENA Mask */
-
-#define MPU_CTRL_ENABLE_Pos                 0                                             /*!< MPU CTRL: ENABLE Position */
-#define MPU_CTRL_ENABLE_Msk                (1UL << MPU_CTRL_ENABLE_Pos)                   /*!< MPU CTRL: ENABLE Mask */
-
-/* MPU Region Number Register */
-#define MPU_RNR_REGION_Pos                  0                                             /*!< MPU RNR: REGION Position */
-#define MPU_RNR_REGION_Msk                 (0xFFUL << MPU_RNR_REGION_Pos)                 /*!< MPU RNR: REGION Mask */
-
-/* MPU Region Base Address Register */
-#define MPU_RBAR_ADDR_Pos                   5                                             /*!< MPU RBAR: ADDR Position */
-#define MPU_RBAR_ADDR_Msk                  (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos)             /*!< MPU RBAR: ADDR Mask */
-
-#define MPU_RBAR_VALID_Pos                  4                                             /*!< MPU RBAR: VALID Position */
-#define MPU_RBAR_VALID_Msk                 (1UL << MPU_RBAR_VALID_Pos)                    /*!< MPU RBAR: VALID Mask */
-
-#define MPU_RBAR_REGION_Pos                 0                                             /*!< MPU RBAR: REGION Position */
-#define MPU_RBAR_REGION_Msk                (0xFUL << MPU_RBAR_REGION_Pos)                 /*!< MPU RBAR: REGION Mask */
-
-/* MPU Region Attribute and Size Register */
-#define MPU_RASR_ATTRS_Pos                 16                                             /*!< MPU RASR: MPU Region Attribute field Position */
-#define MPU_RASR_ATTRS_Msk                 (0xFFFFUL << MPU_RASR_ATTRS_Pos)               /*!< MPU RASR: MPU Region Attribute field Mask */
-
-#define MPU_RASR_XN_Pos                    28                                             /*!< MPU RASR: ATTRS.XN Position */
-#define MPU_RASR_XN_Msk                    (1UL << MPU_RASR_XN_Pos)                       /*!< MPU RASR: ATTRS.XN Mask */
-
-#define MPU_RASR_AP_Pos                    24                                             /*!< MPU RASR: ATTRS.AP Position */
-#define MPU_RASR_AP_Msk                    (0x7UL << MPU_RASR_AP_Pos)                     /*!< MPU RASR: ATTRS.AP Mask */
-
-#define MPU_RASR_TEX_Pos                   19                                             /*!< MPU RASR: ATTRS.TEX Position */
-#define MPU_RASR_TEX_Msk                   (0x7UL << MPU_RASR_TEX_Pos)                    /*!< MPU RASR: ATTRS.TEX Mask */
-
-#define MPU_RASR_S_Pos                     18                                             /*!< MPU RASR: ATTRS.S Position */
-#define MPU_RASR_S_Msk                     (1UL << MPU_RASR_S_Pos)                        /*!< MPU RASR: ATTRS.S Mask */
-
-#define MPU_RASR_C_Pos                     17                                             /*!< MPU RASR: ATTRS.C Position */
-#define MPU_RASR_C_Msk                     (1UL << MPU_RASR_C_Pos)                        /*!< MPU RASR: ATTRS.C Mask */
-
-#define MPU_RASR_B_Pos                     16                                             /*!< MPU RASR: ATTRS.B Position */
-#define MPU_RASR_B_Msk                     (1UL << MPU_RASR_B_Pos)                        /*!< MPU RASR: ATTRS.B Mask */
-
-#define MPU_RASR_SRD_Pos                    8                                             /*!< MPU RASR: Sub-Region Disable Position */
-#define MPU_RASR_SRD_Msk                   (0xFFUL << MPU_RASR_SRD_Pos)                   /*!< MPU RASR: Sub-Region Disable Mask */
-
-#define MPU_RASR_SIZE_Pos                   1                                             /*!< MPU RASR: Region Size Field Position */
-#define MPU_RASR_SIZE_Msk                  (0x1FUL << MPU_RASR_SIZE_Pos)                  /*!< MPU RASR: Region Size Field Mask */
-
-#define MPU_RASR_ENABLE_Pos                 0                                             /*!< MPU RASR: Region enable bit Position */
-#define MPU_RASR_ENABLE_Msk                (1UL << MPU_RASR_ENABLE_Pos)                   /*!< MPU RASR: Region enable bit Disable Mask */
-
-/*@} end of group CMSIS_MPU */
-#endif
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_CoreDebug       Core Debug Registers (CoreDebug)
-    \brief      Type definitions for the Core Debug Registers
-  @{
- */
-
-/** \brief  Structure type to access the Core Debug Register (CoreDebug).
- */
-typedef struct
-{
-  __IO uint32_t DHCSR;                   /*!< Offset: 0x000 (R/W)  Debug Halting Control and Status Register    */
-  __O  uint32_t DCRSR;                   /*!< Offset: 0x004 ( /W)  Debug Core Register Selector Register        */
-  __IO uint32_t DCRDR;                   /*!< Offset: 0x008 (R/W)  Debug Core Register Data Register            */
-  __IO uint32_t DEMCR;                   /*!< Offset: 0x00C (R/W)  Debug Exception and Monitor Control Register */
-} CoreDebug_Type;
-
-/* Debug Halting Control and Status Register */
-#define CoreDebug_DHCSR_DBGKEY_Pos         16                                             /*!< CoreDebug DHCSR: DBGKEY Position */
-#define CoreDebug_DHCSR_DBGKEY_Msk         (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos)       /*!< CoreDebug DHCSR: DBGKEY Mask */
-
-#define CoreDebug_DHCSR_S_RESET_ST_Pos     25                                             /*!< CoreDebug DHCSR: S_RESET_ST Position */
-#define CoreDebug_DHCSR_S_RESET_ST_Msk     (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos)        /*!< CoreDebug DHCSR: S_RESET_ST Mask */
-
-#define CoreDebug_DHCSR_S_RETIRE_ST_Pos    24                                             /*!< CoreDebug DHCSR: S_RETIRE_ST Position */
-#define CoreDebug_DHCSR_S_RETIRE_ST_Msk    (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos)       /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */
-
-#define CoreDebug_DHCSR_S_LOCKUP_Pos       19                                             /*!< CoreDebug DHCSR: S_LOCKUP Position */
-#define CoreDebug_DHCSR_S_LOCKUP_Msk       (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos)          /*!< CoreDebug DHCSR: S_LOCKUP Mask */
-
-#define CoreDebug_DHCSR_S_SLEEP_Pos        18                                             /*!< CoreDebug DHCSR: S_SLEEP Position */
-#define CoreDebug_DHCSR_S_SLEEP_Msk        (1UL << CoreDebug_DHCSR_S_SLEEP_Pos)           /*!< CoreDebug DHCSR: S_SLEEP Mask */
-
-#define CoreDebug_DHCSR_S_HALT_Pos         17                                             /*!< CoreDebug DHCSR: S_HALT Position */
-#define CoreDebug_DHCSR_S_HALT_Msk         (1UL << CoreDebug_DHCSR_S_HALT_Pos)            /*!< CoreDebug DHCSR: S_HALT Mask */
-
-#define CoreDebug_DHCSR_S_REGRDY_Pos       16                                             /*!< CoreDebug DHCSR: S_REGRDY Position */
-#define CoreDebug_DHCSR_S_REGRDY_Msk       (1UL << CoreDebug_DHCSR_S_REGRDY_Pos)          /*!< CoreDebug DHCSR: S_REGRDY Mask */
-
-#define CoreDebug_DHCSR_C_SNAPSTALL_Pos     5                                             /*!< CoreDebug DHCSR: C_SNAPSTALL Position */
-#define CoreDebug_DHCSR_C_SNAPSTALL_Msk    (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos)       /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */
-
-#define CoreDebug_DHCSR_C_MASKINTS_Pos      3                                             /*!< CoreDebug DHCSR: C_MASKINTS Position */
-#define CoreDebug_DHCSR_C_MASKINTS_Msk     (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos)        /*!< CoreDebug DHCSR: C_MASKINTS Mask */
-
-#define CoreDebug_DHCSR_C_STEP_Pos          2                                             /*!< CoreDebug DHCSR: C_STEP Position */
-#define CoreDebug_DHCSR_C_STEP_Msk         (1UL << CoreDebug_DHCSR_C_STEP_Pos)            /*!< CoreDebug DHCSR: C_STEP Mask */
-
-#define CoreDebug_DHCSR_C_HALT_Pos          1                                             /*!< CoreDebug DHCSR: C_HALT Position */
-#define CoreDebug_DHCSR_C_HALT_Msk         (1UL << CoreDebug_DHCSR_C_HALT_Pos)            /*!< CoreDebug DHCSR: C_HALT Mask */
-
-#define CoreDebug_DHCSR_C_DEBUGEN_Pos       0                                             /*!< CoreDebug DHCSR: C_DEBUGEN Position */
-#define CoreDebug_DHCSR_C_DEBUGEN_Msk      (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos)         /*!< CoreDebug DHCSR: C_DEBUGEN Mask */
-
-/* Debug Core Register Selector Register */
-#define CoreDebug_DCRSR_REGWnR_Pos         16                                             /*!< CoreDebug DCRSR: REGWnR Position */
-#define CoreDebug_DCRSR_REGWnR_Msk         (1UL << CoreDebug_DCRSR_REGWnR_Pos)            /*!< CoreDebug DCRSR: REGWnR Mask */
-
-#define CoreDebug_DCRSR_REGSEL_Pos          0                                             /*!< CoreDebug DCRSR: REGSEL Position */
-#define CoreDebug_DCRSR_REGSEL_Msk         (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos)         /*!< CoreDebug DCRSR: REGSEL Mask */
-
-/* Debug Exception and Monitor Control Register */
-#define CoreDebug_DEMCR_TRCENA_Pos         24                                             /*!< CoreDebug DEMCR: TRCENA Position */
-#define CoreDebug_DEMCR_TRCENA_Msk         (1UL << CoreDebug_DEMCR_TRCENA_Pos)            /*!< CoreDebug DEMCR: TRCENA Mask */
-
-#define CoreDebug_DEMCR_MON_REQ_Pos        19                                             /*!< CoreDebug DEMCR: MON_REQ Position */
-#define CoreDebug_DEMCR_MON_REQ_Msk        (1UL << CoreDebug_DEMCR_MON_REQ_Pos)           /*!< CoreDebug DEMCR: MON_REQ Mask */
-
-#define CoreDebug_DEMCR_MON_STEP_Pos       18                                             /*!< CoreDebug DEMCR: MON_STEP Position */
-#define CoreDebug_DEMCR_MON_STEP_Msk       (1UL << CoreDebug_DEMCR_MON_STEP_Pos)          /*!< CoreDebug DEMCR: MON_STEP Mask */
-
-#define CoreDebug_DEMCR_MON_PEND_Pos       17                                             /*!< CoreDebug DEMCR: MON_PEND Position */
-#define CoreDebug_DEMCR_MON_PEND_Msk       (1UL << CoreDebug_DEMCR_MON_PEND_Pos)          /*!< CoreDebug DEMCR: MON_PEND Mask */
-
-#define CoreDebug_DEMCR_MON_EN_Pos         16                                             /*!< CoreDebug DEMCR: MON_EN Position */
-#define CoreDebug_DEMCR_MON_EN_Msk         (1UL << CoreDebug_DEMCR_MON_EN_Pos)            /*!< CoreDebug DEMCR: MON_EN Mask */
-
-#define CoreDebug_DEMCR_VC_HARDERR_Pos     10                                             /*!< CoreDebug DEMCR: VC_HARDERR Position */
-#define CoreDebug_DEMCR_VC_HARDERR_Msk     (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos)        /*!< CoreDebug DEMCR: VC_HARDERR Mask */
-
-#define CoreDebug_DEMCR_VC_INTERR_Pos       9                                             /*!< CoreDebug DEMCR: VC_INTERR Position */
-#define CoreDebug_DEMCR_VC_INTERR_Msk      (1UL << CoreDebug_DEMCR_VC_INTERR_Pos)         /*!< CoreDebug DEMCR: VC_INTERR Mask */
-
-#define CoreDebug_DEMCR_VC_BUSERR_Pos       8                                             /*!< CoreDebug DEMCR: VC_BUSERR Position */
-#define CoreDebug_DEMCR_VC_BUSERR_Msk      (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos)         /*!< CoreDebug DEMCR: VC_BUSERR Mask */
-
-#define CoreDebug_DEMCR_VC_STATERR_Pos      7                                             /*!< CoreDebug DEMCR: VC_STATERR Position */
-#define CoreDebug_DEMCR_VC_STATERR_Msk     (1UL << CoreDebug_DEMCR_VC_STATERR_Pos)        /*!< CoreDebug DEMCR: VC_STATERR Mask */
-
-#define CoreDebug_DEMCR_VC_CHKERR_Pos       6                                             /*!< CoreDebug DEMCR: VC_CHKERR Position */
-#define CoreDebug_DEMCR_VC_CHKERR_Msk      (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos)         /*!< CoreDebug DEMCR: VC_CHKERR Mask */
-
-#define CoreDebug_DEMCR_VC_NOCPERR_Pos      5                                             /*!< CoreDebug DEMCR: VC_NOCPERR Position */
-#define CoreDebug_DEMCR_VC_NOCPERR_Msk     (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos)        /*!< CoreDebug DEMCR: VC_NOCPERR Mask */
-
-#define CoreDebug_DEMCR_VC_MMERR_Pos        4                                             /*!< CoreDebug DEMCR: VC_MMERR Position */
-#define CoreDebug_DEMCR_VC_MMERR_Msk       (1UL << CoreDebug_DEMCR_VC_MMERR_Pos)          /*!< CoreDebug DEMCR: VC_MMERR Mask */
-
-#define CoreDebug_DEMCR_VC_CORERESET_Pos    0                                             /*!< CoreDebug DEMCR: VC_CORERESET Position */
-#define CoreDebug_DEMCR_VC_CORERESET_Msk   (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos)      /*!< CoreDebug DEMCR: VC_CORERESET Mask */
-
-/*@} end of group CMSIS_CoreDebug */
-
-
-/** \ingroup    CMSIS_core_register
-    \defgroup   CMSIS_core_base     Core Definitions
-    \brief      Definitions for base addresses, unions, and structures.
-  @{
- */
-
-/* Memory mapping of Cortex-M3 Hardware */
-#define SCS_BASE            (0xE000E000UL)                            /*!< System Control Space Base Address  */
-#define ITM_BASE            (0xE0000000UL)                            /*!< ITM Base Address                   */
-#define DWT_BASE            (0xE0001000UL)                            /*!< DWT Base Address                   */
-#define TPI_BASE            (0xE0040000UL)                            /*!< TPI Base Address                   */
-#define CoreDebug_BASE      (0xE000EDF0UL)                            /*!< Core Debug Base Address            */
-#define SysTick_BASE        (SCS_BASE +  0x0010UL)                    /*!< SysTick Base Address               */
-#define NVIC_BASE           (SCS_BASE +  0x0100UL)                    /*!< NVIC Base Address                  */
-#define SCB_BASE            (SCS_BASE +  0x0D00UL)                    /*!< System Control Block Base Address  */
-
-#define SCnSCB              ((SCnSCB_Type    *)     SCS_BASE      )   /*!< System control Register not in SCB */
-#define SCB                 ((SCB_Type       *)     SCB_BASE      )   /*!< SCB configuration struct           */
-#define SysTick             ((SysTick_Type   *)     SysTick_BASE  )   /*!< SysTick configuration struct       */
-#define NVIC                ((NVIC_Type      *)     NVIC_BASE     )   /*!< NVIC configuration struct          */
-#define ITM                 ((ITM_Type       *)     ITM_BASE      )   /*!< ITM configuration struct           */
-#define DWT                 ((DWT_Type       *)     DWT_BASE      )   /*!< DWT configuration struct           */
-#define TPI                 ((TPI_Type       *)     TPI_BASE      )   /*!< TPI configuration struct           */
-#define CoreDebug           ((CoreDebug_Type *)     CoreDebug_BASE)   /*!< Core Debug configuration struct    */
-
-#if (__MPU_PRESENT == 1)
-  #define MPU_BASE          (SCS_BASE +  0x0D90UL)                    /*!< Memory Protection Unit             */
-  #define MPU               ((MPU_Type       *)     MPU_BASE      )   /*!< Memory Protection Unit             */
-#endif
-
-/*@} */
-
-
-
-/*******************************************************************************
- *                Hardware Abstraction Layer
-  Core Function Interface contains:
-  - Core NVIC Functions
-  - Core SysTick Functions
-  - Core Debug Functions
-  - Core Register Access Functions
- ******************************************************************************/
-/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
-*/
-
-
-
-/* ##########################   NVIC functions  #################################### */
-/** \ingroup  CMSIS_Core_FunctionInterface
-    \defgroup CMSIS_Core_NVICFunctions NVIC Functions
-    \brief      Functions that manage interrupts and exceptions via the NVIC.
-    @{
- */
-
-/** \brief  Set Priority Grouping
-
-  The function sets the priority grouping field using the required unlock sequence.
-  The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.
-  Only values from 0..7 are used.
-  In case of a conflict between priority grouping and available
-  priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
-
-    \param [in]      PriorityGroup  Priority grouping field.
- */
-__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
-{
-  uint32_t reg_value;
-  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07);               /* only values 0..7 are used          */
-
-  reg_value  =  SCB->AIRCR;                                                   /* read old register configuration    */
-  reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk);             /* clear bits to change               */
-  reg_value  =  (reg_value                                 |
-                ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |
-                (PriorityGroupTmp << 8));                                     /* Insert write key and priorty group */
-  SCB->AIRCR =  reg_value;
-}
-
-
-/** \brief  Get Priority Grouping
-
-  The function reads the priority grouping field from the NVIC Interrupt Controller.
-
-    \return                Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).
- */
-__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
-{
-  return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos);   /* read priority grouping field */
-}
-
-
-/** \brief  Enable External Interrupt
-
-    The function enables a device-specific interrupt in the NVIC interrupt controller.
-
-    \param [in]      IRQn  External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
-{
-  NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* enable interrupt */
-}
-
-
-/** \brief  Disable External Interrupt
-
-    The function disables a device-specific interrupt in the NVIC interrupt controller.
-
-    \param [in]      IRQn  External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
-{
-  NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */
-}
-
-
-/** \brief  Get Pending Interrupt
-
-    The function reads the pending register in the NVIC and returns the pending bit
-    for the specified interrupt.
-
-    \param [in]      IRQn  Interrupt number.
-
-    \return             0  Interrupt status is not pending.
-    \return             1  Interrupt status is pending.
- */
-__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
-{
-  return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */
-}
-
-
-/** \brief  Set Pending Interrupt
-
-    The function sets the pending bit of an external interrupt.
-
-    \param [in]      IRQn  Interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
-{
-  NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */
-}
-
-
-/** \brief  Clear Pending Interrupt
-
-    The function clears the pending bit of an external interrupt.
-
-    \param [in]      IRQn  External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
-{
-  NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
-}
-
-
-/** \brief  Get Active Interrupt
-
-    The function reads the active register in NVIC and returns the active bit.
-
-    \param [in]      IRQn  Interrupt number.
-
-    \return             0  Interrupt status is not active.
-    \return             1  Interrupt status is active.
- */
-__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
-{
-  return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */
-}
-
-
-/** \brief  Set Interrupt Priority
-
-    The function sets the priority of an interrupt.
-
-    \note The priority cannot be set for every core interrupt.
-
-    \param [in]      IRQn  Interrupt number.
-    \param [in]  priority  Priority to set.
- */
-__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
-{
-  if(IRQn < 0) {
-    SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M  System Interrupts */
-  else {
-    NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff);    }        /* set Priority for device specific Interrupts  */
-}
-
-
-/** \brief  Get Interrupt Priority
-
-    The function reads the priority of an interrupt. The interrupt
-    number can be positive to specify an external (device specific)
-    interrupt, or negative to specify an internal (core) interrupt.
-
-
-    \param [in]   IRQn  Interrupt number.
-    \return             Interrupt Priority. Value is aligned automatically to the implemented
-                        priority bits of the microcontroller.
- */
-__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
-{
-
-  if(IRQn < 0) {
-    return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for Cortex-M  system interrupts */
-  else {
-    return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)]           >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for device specific interrupts  */
-}
-
-
-/** \brief  Encode Priority
-
-    The function encodes the priority for an interrupt with the given priority group,
-    preemptive priority value, and subpriority value.
-    In case of a conflict between priority grouping and available
-    priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.
-
-    \param [in]     PriorityGroup  Used priority group.
-    \param [in]   PreemptPriority  Preemptive priority value (starting from 0).
-    \param [in]       SubPriority  Subpriority value (starting from 0).
-    \return                        Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
- */
-__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
-{
-  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */
-  uint32_t PreemptPriorityBits;
-  uint32_t SubPriorityBits;
-
-  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
-  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
-
-  return (
-           ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |
-           ((SubPriority     & ((1 << (SubPriorityBits    )) - 1)))
-         );
-}
-
-
-/** \brief  Decode Priority
-
-    The function decodes an interrupt priority value with a given priority group to
-    preemptive priority value and subpriority value.
-    In case of a conflict between priority grouping and available
-    priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.
-
-    \param [in]         Priority   Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
-    \param [in]     PriorityGroup  Used priority group.
-    \param [out] pPreemptPriority  Preemptive priority value (starting from 0).
-    \param [out]     pSubPriority  Subpriority value (starting from 0).
- */
-__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)
-{
-  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */
-  uint32_t PreemptPriorityBits;
-  uint32_t SubPriorityBits;
-
-  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
-  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
-
-  *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);
-  *pSubPriority     = (Priority                   ) & ((1 << (SubPriorityBits    )) - 1);
-}
-
-
-/** \brief  System Reset
-
-    The function initiates a system reset request to reset the MCU.
- */
-__STATIC_INLINE void NVIC_SystemReset(void)
-{
-  __DSB();                                                     /* Ensure all outstanding memory accesses included
-                                                                  buffered write are completed before reset */
-  SCB->AIRCR  = ((0x5FA << SCB_AIRCR_VECTKEY_Pos)      |
-                 (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
-                 SCB_AIRCR_SYSRESETREQ_Msk);                   /* Keep priority group unchanged */
-  __DSB();                                                     /* Ensure completion of memory access */
-  while(1);                                                    /* wait until reset */
-}
-
-/*@} end of CMSIS_Core_NVICFunctions */
-
-
-
-/* ##################################    SysTick function  ############################################ */
-/** \ingroup  CMSIS_Core_FunctionInterface
-    \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
-    \brief      Functions that configure the System.
-  @{
- */
-
-#if (__Vendor_SysTickConfig == 0)
-
-/** \brief  System Tick Configuration
-
-    The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
-    Counter is in free running mode to generate periodic interrupts.
-
-    \param [in]  ticks  Number of ticks between two interrupts.
-
-    \return          0  Function succeeded.
-    \return          1  Function failed.
-
-    \note     When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
-    function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
-    must contain a vendor-specific implementation of this function.
-
- */
-__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
-{
-  if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk)  return (1);      /* Reload value impossible */
-
-  SysTick->LOAD  = ticks - 1;                                  /* set reload register */
-  NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1);  /* set Priority for Systick Interrupt */
-  SysTick->VAL   = 0;                                          /* Load the SysTick Counter Value */
-  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |
-                   SysTick_CTRL_TICKINT_Msk   |
-                   SysTick_CTRL_ENABLE_Msk;                    /* Enable SysTick IRQ and SysTick Timer */
-  return (0);                                                  /* Function successful */
-}
-
-#endif
-
-/*@} end of CMSIS_Core_SysTickFunctions */
-
-
-
-/* ##################################### Debug In/Output function ########################################### */
-/** \ingroup  CMSIS_Core_FunctionInterface
-    \defgroup CMSIS_core_DebugFunctions ITM Functions
-    \brief   Functions that access the ITM debug interface.
-  @{
- */
-
-extern volatile int32_t ITM_RxBuffer;                    /*!< External variable to receive characters.                         */
-#define                 ITM_RXBUFFER_EMPTY    0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */
-
-
-/** \brief  ITM Send Character
-
-    The function transmits a character via the ITM channel 0, and
-    \li Just returns when no debugger is connected that has booked the output.
-    \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.
-
-    \param [in]     ch  Character to transmit.
-
-    \returns            Character to transmit.
- */
-__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
-{
-  if ((ITM->TCR & ITM_TCR_ITMENA_Msk)                  &&      /* ITM enabled */
-      (ITM->TER & (1UL << 0)        )                    )     /* ITM Port #0 enabled */
-  {
-    while (ITM->PORT[0].u32 == 0);
-    ITM->PORT[0].u8 = (uint8_t) ch;
-  }
-  return (ch);
-}
-
-
-/** \brief  ITM Receive Character
-
-    The function inputs a character via the external variable \ref ITM_RxBuffer.
-
-    \return             Received character.
-    \return         -1  No character pending.
- */
-__STATIC_INLINE int32_t ITM_ReceiveChar (void) {
-  int32_t ch = -1;                           /* no character available */
-
-  if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {
-    ch = ITM_RxBuffer;
-    ITM_RxBuffer = ITM_RXBUFFER_EMPTY;       /* ready for next character */
-  }
-
-  return (ch);
-}
-
-
-/** \brief  ITM Check Character
-
-    The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.
-
-    \return          0  No character available.
-    \return          1  Character available.
- */
-__STATIC_INLINE int32_t ITM_CheckChar (void) {
-
-  if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {
-    return (0);                                 /* no character available */
-  } else {
-    return (1);                                 /*    character available */
-  }
-}
-
-/*@} end of CMSIS_core_DebugFunctions */
-
-#endif /* __CORE_CM3_H_DEPENDANT */
-
-#endif /* __CMSIS_GENERIC */
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/modules/mathlib/CMSIS/Include/core_cm4.h
deleted file mode 100644
index 93efd3a7a..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cm4.h
+++ /dev/null
@@ -1,1772 +0,0 @@
-/**************************************************************************//**
- * @file     core_cm4.h
- * @brief    CMSIS Cortex-M4 Core Peripheral Access Layer Header File
- * @version  V3.20
- * @date     25. February 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
-   All rights reserved.
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-   - Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   - 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.
-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
-   ---------------------------------------------------------------------------*/
-
-
-#if defined ( __ICCARM__ )
- #pragma system_include  /* treat file as system include file for MISRA check */
-#endif
-
-#ifdef __cplusplus
- extern "C" {
-#endif
-
-#ifndef __CORE_CM4_H_GENERIC
-#define __CORE_CM4_H_GENERIC
-
-/** \page CMSIS_MISRA_Exceptions  MISRA-C:2004 Compliance Exceptions
-  CMSIS violates the following MISRA-C:2004 rules:
-
-   \li Required Rule 8.5, object/function definition in header file.<br>
-     Function definitions in header files are used to allow 'inlining'.
-
-   \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
-     Unions are used for effective representation of core registers.
-
-   \li Advisory Rule 19.7, Function-like macro defined.<br>
-     Function-like macros are used to allow more efficient code.
- */
-
-
-/*******************************************************************************
- *                 CMSIS definitions
- ******************************************************************************/
-/** \ingroup Cortex_M4
-  @{
- */
-
-/*  CMSIS CM4 definitions */
-#define __CM4_CMSIS_VERSION_MAIN  (0x03)                                   /*!< [31:16] CMSIS HAL main version   */
-#define __CM4_CMSIS_VERSION_SUB   (0x20)                                   /*!< [15:0]  CMSIS HAL sub version    */
-#define __CM4_CMSIS_VERSION       ((__CM4_CMSIS_VERSION_MAIN << 16) | \
-                                    __CM4_CMSIS_VERSION_SUB          )     /*!< CMSIS HAL version number         */
-
-#define __CORTEX_M                (0x04)                                   /*!< Cortex-M Core                    */
-
-
-#if   defined ( __CC_ARM )
-  #define __ASM            __asm                                      /*!< asm keyword for ARM Compiler          */
-  #define __INLINE         __inline                                   /*!< inline keyword for ARM Compiler       */
-  #define __STATIC_INLINE  static __inline
-
-#elif defined ( __ICCARM__ )
-  #define __ASM            __asm                                      /*!< asm keyword for IAR Compiler          */
-  #define __INLINE         inline                                     /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */
-  #define __STATIC_INLINE  static inline
-
-#elif defined ( __TMS470__ )
-  #define __ASM            __asm                                      /*!< asm keyword for TI CCS Compiler       */
-  #define __STATIC_INLINE  static inline
-
-#elif defined ( __GNUC__ )
-  #define __ASM            __asm                                      /*!< asm keyword for GNU Compiler          */
-  #define __INLINE         inline                                     /*!< inline keyword for GNU Compiler       */
-  #define __STATIC_INLINE  static inline
-
-#elif defined ( __TASKING__ )
-  #define __ASM            __asm                                      /*!< asm keyword for TASKING Compiler      */
-  #define __INLINE         inline                                     /*!< inline keyword for TASKING Compiler   */
-  #define __STATIC_INLINE  static inline
-
-#endif
-
-/** __FPU_USED indicates whether an FPU is used or not. For this, __FPU_PRESENT has to be checked prior to making use of FPU specific registers and functions.
-*/
-#if defined ( __CC_ARM )
-  #if defined __TARGET_FPU_VFP
-    #if (__FPU_PRESENT == 1)
-      #define __FPU_USED       1
-    #else
-      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-      #define __FPU_USED       0
-    #endif
-  #else
-    #define __FPU_USED         0
-  #endif
-
-#elif defined ( __ICCARM__ )
-  #if defined __ARMVFP__
-    #if (__FPU_PRESENT == 1)
-      #define __FPU_USED       1
-    #else
-      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-      #define __FPU_USED       0
-    #endif
-  #else
-    #define __FPU_USED         0
-  #endif
-
-#elif defined ( __TMS470__ )
-  #if defined __TI_VFP_SUPPORT__
-    #if (__FPU_PRESENT == 1)
-      #define __FPU_USED       1
-    #else
-      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-      #define __FPU_USED       0
-    #endif
-  #else
-    #define __FPU_USED         0
-  #endif
-
-#elif defined ( __GNUC__ )
-  #if defined (__VFP_FP__) && !defined(__SOFTFP__)
-    #if (__FPU_PRESENT == 1)
-      #define __FPU_USED       1
-    #else
-      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-      #define __FPU_USED       0
-    #endif
-  #else
-    #define __FPU_USED         0
-  #endif
-
-#elif defined ( __TASKING__ )
-  #if defined __FPU_VFP__
-    #if (__FPU_PRESENT == 1)
-      #define __FPU_USED       1
-    #else
-      #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
-      #define __FPU_USED       0
-    #endif
-  #else
-    #define __FPU_USED         0
-  #endif
-#endif
-
-#include <stdint.h>                      /* standard types definitions                      */
-#include "core_cmInstr.h"                /* Core Instruction Access                         */
-#include "core_cmFunc.h"                 /* Core Function Access                            */
-#include "core_cm4_simd.h"               /* Compiler specific SIMD Intrinsics               */
-
-#endif /* __CORE_CM4_H_GENERIC */
-
-#ifndef __CMSIS_GENERIC
-
-#ifndef __CORE_CM4_H_DEPENDANT
-#define __CORE_CM4_H_DEPENDANT
-
-/* check device defines and use defaults */
-#if defined __CHECK_DEVICE_DEFINES
-  #ifndef __CM4_REV
-    #define __CM4_REV               0x0000
-    #warning "__CM4_REV not defined in device header file; using default!"
-  #endif
-
-  #ifndef __FPU_PRESENT
-    #define __FPU_PRESENT             0
-    #warning "__FPU_PRESENT not defined in device header file; using default!"
-  #endif
-
-  #ifndef __MPU_PRESENT
-    #define __MPU_PRESENT             0
-    #warning "__MPU_PRESENT not defined in device header file; using default!"
-  #endif
-
-  #ifndef __NVIC_PRIO_BITS
-    #define __NVIC_PRIO_BITS          4
-    #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
-  #endif
-
-  #ifndef __Vendor_SysTickConfig
-    #define __Vendor_SysTickConfig    0
-    #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
-  #endif
-#endif
-
-/* IO definitions (access restrictions to peripheral registers) */
-/**
-    \defgroup CMSIS_glob_defs CMSIS Global Defines
-
-    <strong>IO Type Qualifiers</strong> are used
-    \li to specify the access to peripheral variables.
-    \li for automatic generation of peripheral register debug information.
-*/
-#ifdef __cplusplus
-  #define   __I     volatile             /*!< Defines 'read only' permissions                 */
-#else
-  #define   __I     volatile const       /*!< Defines 'read only' permissions                 */
-#endif
-#define     __O     volatile             /*!< Defines 'write only' permissions                */
-#define     __IO    volatile             /*!< Defines 'read / write' permissions              */
-
-/*@} end of group Cortex_M4 */
-
-
-
-/*******************************************************************************
- *                 Register Abstraction
-  Core Register contain:
-  - Core Register
-  - Core NVIC Register
-  - Core SCB Register
-  - Core SysTick Register
-  - Core Debug Register
-  - Core MPU Register
-  - Core FPU Register
- ******************************************************************************/
-/** \defgroup CMSIS_core_register Defines and Type Definitions
-    \brief Type definitions and defines for Cortex-M processor based devices.
-*/
-
-/** \ingroup    CMSIS_core_register
-    \defgroup   CMSIS_CORE  Status and Control Registers
-    \brief  Core Register type definitions.
-  @{
- */
-
-/** \brief  Union type to access the Application Program Status Register (APSR).
- */
-typedef union
-{
-  struct
-  {
-#if (__CORTEX_M != 0x04)
-    uint32_t _reserved0:27;              /*!< bit:  0..26  Reserved                           */
-#else
-    uint32_t _reserved0:16;              /*!< bit:  0..15  Reserved                           */
-    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */
-    uint32_t _reserved1:7;               /*!< bit: 20..26  Reserved                           */
-#endif
-    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */
-    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */
-    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */
-    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */
-    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */
-  } b;                                   /*!< Structure used for bit  access                  */
-  uint32_t w;                            /*!< Type      used for word access                  */
-} APSR_Type;
-
-
-/** \brief  Union type to access the Interrupt Program Status Register (IPSR).
- */
-typedef union
-{
-  struct
-  {
-    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */
-    uint32_t _reserved0:23;              /*!< bit:  9..31  Reserved                           */
-  } b;                                   /*!< Structure used for bit  access                  */
-  uint32_t w;                            /*!< Type      used for word access                  */
-} IPSR_Type;
-
-
-/** \brief  Union type to access the Special-Purpose Program Status Registers (xPSR).
- */
-typedef union
-{
-  struct
-  {
-    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */
-#if (__CORTEX_M != 0x04)
-    uint32_t _reserved0:15;              /*!< bit:  9..23  Reserved                           */
-#else
-    uint32_t _reserved0:7;               /*!< bit:  9..15  Reserved                           */
-    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */
-    uint32_t _reserved1:4;               /*!< bit: 20..23  Reserved                           */
-#endif
-    uint32_t T:1;                        /*!< bit:     24  Thumb bit        (read 0)          */
-    uint32_t IT:2;                       /*!< bit: 25..26  saved IT state   (read 0)          */
-    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */
-    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */
-    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */
-    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */
-    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */
-  } b;                                   /*!< Structure used for bit  access                  */
-  uint32_t w;                            /*!< Type      used for word access                  */
-} xPSR_Type;
-
-
-/** \brief  Union type to access the Control Registers (CONTROL).
- */
-typedef union
-{
-  struct
-  {
-    uint32_t nPRIV:1;                    /*!< bit:      0  Execution privilege in Thread mode */
-    uint32_t SPSEL:1;                    /*!< bit:      1  Stack to be used                   */
-    uint32_t FPCA:1;                     /*!< bit:      2  FP extension active flag           */
-    uint32_t _reserved0:29;              /*!< bit:  3..31  Reserved                           */
-  } b;                                   /*!< Structure used for bit  access                  */
-  uint32_t w;                            /*!< Type      used for word access                  */
-} CONTROL_Type;
-
-/*@} end of group CMSIS_CORE */
-
-
-/** \ingroup    CMSIS_core_register
-    \defgroup   CMSIS_NVIC  Nested Vectored Interrupt Controller (NVIC)
-    \brief      Type definitions for the NVIC Registers
-  @{
- */
-
-/** \brief  Structure type to access the Nested Vectored Interrupt Controller (NVIC).
- */
-typedef struct
-{
-  __IO uint32_t ISER[8];                 /*!< Offset: 0x000 (R/W)  Interrupt Set Enable Register           */
-       uint32_t RESERVED0[24];
-  __IO uint32_t ICER[8];                 /*!< Offset: 0x080 (R/W)  Interrupt Clear Enable Register         */
-       uint32_t RSERVED1[24];
-  __IO uint32_t ISPR[8];                 /*!< Offset: 0x100 (R/W)  Interrupt Set Pending Register          */
-       uint32_t RESERVED2[24];
-  __IO uint32_t ICPR[8];                 /*!< Offset: 0x180 (R/W)  Interrupt Clear Pending Register        */
-       uint32_t RESERVED3[24];
-  __IO uint32_t IABR[8];                 /*!< Offset: 0x200 (R/W)  Interrupt Active bit Register           */
-       uint32_t RESERVED4[56];
-  __IO uint8_t  IP[240];                 /*!< Offset: 0x300 (R/W)  Interrupt Priority Register (8Bit wide) */
-       uint32_t RESERVED5[644];
-  __O  uint32_t STIR;                    /*!< Offset: 0xE00 ( /W)  Software Trigger Interrupt Register     */
-}  NVIC_Type;
-
-/* Software Triggered Interrupt Register Definitions */
-#define NVIC_STIR_INTID_Pos                 0                                          /*!< STIR: INTLINESNUM Position */
-#define NVIC_STIR_INTID_Msk                (0x1FFUL << NVIC_STIR_INTID_Pos)            /*!< STIR: INTLINESNUM Mask */
-
-/*@} end of group CMSIS_NVIC */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_SCB     System Control Block (SCB)
-    \brief      Type definitions for the System Control Block Registers
-  @{
- */
-
-/** \brief  Structure type to access the System Control Block (SCB).
- */
-typedef struct
-{
-  __I  uint32_t CPUID;                   /*!< Offset: 0x000 (R/ )  CPUID Base Register                                   */
-  __IO uint32_t ICSR;                    /*!< Offset: 0x004 (R/W)  Interrupt Control and State Register                  */
-  __IO uint32_t VTOR;                    /*!< Offset: 0x008 (R/W)  Vector Table Offset Register                          */
-  __IO uint32_t AIRCR;                   /*!< Offset: 0x00C (R/W)  Application Interrupt and Reset Control Register      */
-  __IO uint32_t SCR;                     /*!< Offset: 0x010 (R/W)  System Control Register                               */
-  __IO uint32_t CCR;                     /*!< Offset: 0x014 (R/W)  Configuration Control Register                        */
-  __IO uint8_t  SHP[12];                 /*!< Offset: 0x018 (R/W)  System Handlers Priority Registers (4-7, 8-11, 12-15) */
-  __IO uint32_t SHCSR;                   /*!< Offset: 0x024 (R/W)  System Handler Control and State Register             */
-  __IO uint32_t CFSR;                    /*!< Offset: 0x028 (R/W)  Configurable Fault Status Register                    */
-  __IO uint32_t HFSR;                    /*!< Offset: 0x02C (R/W)  HardFault Status Register                             */
-  __IO uint32_t DFSR;                    /*!< Offset: 0x030 (R/W)  Debug Fault Status Register                           */
-  __IO uint32_t MMFAR;                   /*!< Offset: 0x034 (R/W)  MemManage Fault Address Register                      */
-  __IO uint32_t BFAR;                    /*!< Offset: 0x038 (R/W)  BusFault Address Register                             */
-  __IO uint32_t AFSR;                    /*!< Offset: 0x03C (R/W)  Auxiliary Fault Status Register                       */
-  __I  uint32_t PFR[2];                  /*!< Offset: 0x040 (R/ )  Processor Feature Register                            */
-  __I  uint32_t DFR;                     /*!< Offset: 0x048 (R/ )  Debug Feature Register                                */
-  __I  uint32_t ADR;                     /*!< Offset: 0x04C (R/ )  Auxiliary Feature Register                            */
-  __I  uint32_t MMFR[4];                 /*!< Offset: 0x050 (R/ )  Memory Model Feature Register                         */
-  __I  uint32_t ISAR[5];                 /*!< Offset: 0x060 (R/ )  Instruction Set Attributes Register                   */
-       uint32_t RESERVED0[5];
-  __IO uint32_t CPACR;                   /*!< Offset: 0x088 (R/W)  Coprocessor Access Control Register                   */
-} SCB_Type;
-
-/* SCB CPUID Register Definitions */
-#define SCB_CPUID_IMPLEMENTER_Pos          24                                             /*!< SCB CPUID: IMPLEMENTER Position */
-#define SCB_CPUID_IMPLEMENTER_Msk          (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos)          /*!< SCB CPUID: IMPLEMENTER Mask */
-
-#define SCB_CPUID_VARIANT_Pos              20                                             /*!< SCB CPUID: VARIANT Position */
-#define SCB_CPUID_VARIANT_Msk              (0xFUL << SCB_CPUID_VARIANT_Pos)               /*!< SCB CPUID: VARIANT Mask */
-
-#define SCB_CPUID_ARCHITECTURE_Pos         16                                             /*!< SCB CPUID: ARCHITECTURE Position */
-#define SCB_CPUID_ARCHITECTURE_Msk         (0xFUL << SCB_CPUID_ARCHITECTURE_Pos)          /*!< SCB CPUID: ARCHITECTURE Mask */
-
-#define SCB_CPUID_PARTNO_Pos                4                                             /*!< SCB CPUID: PARTNO Position */
-#define SCB_CPUID_PARTNO_Msk               (0xFFFUL << SCB_CPUID_PARTNO_Pos)              /*!< SCB CPUID: PARTNO Mask */
-
-#define SCB_CPUID_REVISION_Pos              0                                             /*!< SCB CPUID: REVISION Position */
-#define SCB_CPUID_REVISION_Msk             (0xFUL << SCB_CPUID_REVISION_Pos)              /*!< SCB CPUID: REVISION Mask */
-
-/* SCB Interrupt Control State Register Definitions */
-#define SCB_ICSR_NMIPENDSET_Pos            31                                             /*!< SCB ICSR: NMIPENDSET Position */
-#define SCB_ICSR_NMIPENDSET_Msk            (1UL << SCB_ICSR_NMIPENDSET_Pos)               /*!< SCB ICSR: NMIPENDSET Mask */
-
-#define SCB_ICSR_PENDSVSET_Pos             28                                             /*!< SCB ICSR: PENDSVSET Position */
-#define SCB_ICSR_PENDSVSET_Msk             (1UL << SCB_ICSR_PENDSVSET_Pos)                /*!< SCB ICSR: PENDSVSET Mask */
-
-#define SCB_ICSR_PENDSVCLR_Pos             27                                             /*!< SCB ICSR: PENDSVCLR Position */
-#define SCB_ICSR_PENDSVCLR_Msk             (1UL << SCB_ICSR_PENDSVCLR_Pos)                /*!< SCB ICSR: PENDSVCLR Mask */
-
-#define SCB_ICSR_PENDSTSET_Pos             26                                             /*!< SCB ICSR: PENDSTSET Position */
-#define SCB_ICSR_PENDSTSET_Msk             (1UL << SCB_ICSR_PENDSTSET_Pos)                /*!< SCB ICSR: PENDSTSET Mask */
-
-#define SCB_ICSR_PENDSTCLR_Pos             25                                             /*!< SCB ICSR: PENDSTCLR Position */
-#define SCB_ICSR_PENDSTCLR_Msk             (1UL << SCB_ICSR_PENDSTCLR_Pos)                /*!< SCB ICSR: PENDSTCLR Mask */
-
-#define SCB_ICSR_ISRPREEMPT_Pos            23                                             /*!< SCB ICSR: ISRPREEMPT Position */
-#define SCB_ICSR_ISRPREEMPT_Msk            (1UL << SCB_ICSR_ISRPREEMPT_Pos)               /*!< SCB ICSR: ISRPREEMPT Mask */
-
-#define SCB_ICSR_ISRPENDING_Pos            22                                             /*!< SCB ICSR: ISRPENDING Position */
-#define SCB_ICSR_ISRPENDING_Msk            (1UL << SCB_ICSR_ISRPENDING_Pos)               /*!< SCB ICSR: ISRPENDING Mask */
-
-#define SCB_ICSR_VECTPENDING_Pos           12                                             /*!< SCB ICSR: VECTPENDING Position */
-#define SCB_ICSR_VECTPENDING_Msk           (0x1FFUL << SCB_ICSR_VECTPENDING_Pos)          /*!< SCB ICSR: VECTPENDING Mask */
-
-#define SCB_ICSR_RETTOBASE_Pos             11                                             /*!< SCB ICSR: RETTOBASE Position */
-#define SCB_ICSR_RETTOBASE_Msk             (1UL << SCB_ICSR_RETTOBASE_Pos)                /*!< SCB ICSR: RETTOBASE Mask */
-
-#define SCB_ICSR_VECTACTIVE_Pos             0                                             /*!< SCB ICSR: VECTACTIVE Position */
-#define SCB_ICSR_VECTACTIVE_Msk            (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos)           /*!< SCB ICSR: VECTACTIVE Mask */
-
-/* SCB Vector Table Offset Register Definitions */
-#define SCB_VTOR_TBLOFF_Pos                 7                                             /*!< SCB VTOR: TBLOFF Position */
-#define SCB_VTOR_TBLOFF_Msk                (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos)           /*!< SCB VTOR: TBLOFF Mask */
-
-/* SCB Application Interrupt and Reset Control Register Definitions */
-#define SCB_AIRCR_VECTKEY_Pos              16                                             /*!< SCB AIRCR: VECTKEY Position */
-#define SCB_AIRCR_VECTKEY_Msk              (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos)            /*!< SCB AIRCR: VECTKEY Mask */
-
-#define SCB_AIRCR_VECTKEYSTAT_Pos          16                                             /*!< SCB AIRCR: VECTKEYSTAT Position */
-#define SCB_AIRCR_VECTKEYSTAT_Msk          (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos)        /*!< SCB AIRCR: VECTKEYSTAT Mask */
-
-#define SCB_AIRCR_ENDIANESS_Pos            15                                             /*!< SCB AIRCR: ENDIANESS Position */
-#define SCB_AIRCR_ENDIANESS_Msk            (1UL << SCB_AIRCR_ENDIANESS_Pos)               /*!< SCB AIRCR: ENDIANESS Mask */
-
-#define SCB_AIRCR_PRIGROUP_Pos              8                                             /*!< SCB AIRCR: PRIGROUP Position */
-#define SCB_AIRCR_PRIGROUP_Msk             (7UL << SCB_AIRCR_PRIGROUP_Pos)                /*!< SCB AIRCR: PRIGROUP Mask */
-
-#define SCB_AIRCR_SYSRESETREQ_Pos           2                                             /*!< SCB AIRCR: SYSRESETREQ Position */
-#define SCB_AIRCR_SYSRESETREQ_Msk          (1UL << SCB_AIRCR_SYSRESETREQ_Pos)             /*!< SCB AIRCR: SYSRESETREQ Mask */
-
-#define SCB_AIRCR_VECTCLRACTIVE_Pos         1                                             /*!< SCB AIRCR: VECTCLRACTIVE Position */
-#define SCB_AIRCR_VECTCLRACTIVE_Msk        (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos)           /*!< SCB AIRCR: VECTCLRACTIVE Mask */
-
-#define SCB_AIRCR_VECTRESET_Pos             0                                             /*!< SCB AIRCR: VECTRESET Position */
-#define SCB_AIRCR_VECTRESET_Msk            (1UL << SCB_AIRCR_VECTRESET_Pos)               /*!< SCB AIRCR: VECTRESET Mask */
-
-/* SCB System Control Register Definitions */
-#define SCB_SCR_SEVONPEND_Pos               4                                             /*!< SCB SCR: SEVONPEND Position */
-#define SCB_SCR_SEVONPEND_Msk              (1UL << SCB_SCR_SEVONPEND_Pos)                 /*!< SCB SCR: SEVONPEND Mask */
-
-#define SCB_SCR_SLEEPDEEP_Pos               2                                             /*!< SCB SCR: SLEEPDEEP Position */
-#define SCB_SCR_SLEEPDEEP_Msk              (1UL << SCB_SCR_SLEEPDEEP_Pos)                 /*!< SCB SCR: SLEEPDEEP Mask */
-
-#define SCB_SCR_SLEEPONEXIT_Pos             1                                             /*!< SCB SCR: SLEEPONEXIT Position */
-#define SCB_SCR_SLEEPONEXIT_Msk            (1UL << SCB_SCR_SLEEPONEXIT_Pos)               /*!< SCB SCR: SLEEPONEXIT Mask */
-
-/* SCB Configuration Control Register Definitions */
-#define SCB_CCR_STKALIGN_Pos                9                                             /*!< SCB CCR: STKALIGN Position */
-#define SCB_CCR_STKALIGN_Msk               (1UL << SCB_CCR_STKALIGN_Pos)                  /*!< SCB CCR: STKALIGN Mask */
-
-#define SCB_CCR_BFHFNMIGN_Pos               8                                             /*!< SCB CCR: BFHFNMIGN Position */
-#define SCB_CCR_BFHFNMIGN_Msk              (1UL << SCB_CCR_BFHFNMIGN_Pos)                 /*!< SCB CCR: BFHFNMIGN Mask */
-
-#define SCB_CCR_DIV_0_TRP_Pos               4                                             /*!< SCB CCR: DIV_0_TRP Position */
-#define SCB_CCR_DIV_0_TRP_Msk              (1UL << SCB_CCR_DIV_0_TRP_Pos)                 /*!< SCB CCR: DIV_0_TRP Mask */
-
-#define SCB_CCR_UNALIGN_TRP_Pos             3                                             /*!< SCB CCR: UNALIGN_TRP Position */
-#define SCB_CCR_UNALIGN_TRP_Msk            (1UL << SCB_CCR_UNALIGN_TRP_Pos)               /*!< SCB CCR: UNALIGN_TRP Mask */
-
-#define SCB_CCR_USERSETMPEND_Pos            1                                             /*!< SCB CCR: USERSETMPEND Position */
-#define SCB_CCR_USERSETMPEND_Msk           (1UL << SCB_CCR_USERSETMPEND_Pos)              /*!< SCB CCR: USERSETMPEND Mask */
-
-#define SCB_CCR_NONBASETHRDENA_Pos          0                                             /*!< SCB CCR: NONBASETHRDENA Position */
-#define SCB_CCR_NONBASETHRDENA_Msk         (1UL << SCB_CCR_NONBASETHRDENA_Pos)            /*!< SCB CCR: NONBASETHRDENA Mask */
-
-/* SCB System Handler Control and State Register Definitions */
-#define SCB_SHCSR_USGFAULTENA_Pos          18                                             /*!< SCB SHCSR: USGFAULTENA Position */
-#define SCB_SHCSR_USGFAULTENA_Msk          (1UL << SCB_SHCSR_USGFAULTENA_Pos)             /*!< SCB SHCSR: USGFAULTENA Mask */
-
-#define SCB_SHCSR_BUSFAULTENA_Pos          17                                             /*!< SCB SHCSR: BUSFAULTENA Position */
-#define SCB_SHCSR_BUSFAULTENA_Msk          (1UL << SCB_SHCSR_BUSFAULTENA_Pos)             /*!< SCB SHCSR: BUSFAULTENA Mask */
-
-#define SCB_SHCSR_MEMFAULTENA_Pos          16                                             /*!< SCB SHCSR: MEMFAULTENA Position */
-#define SCB_SHCSR_MEMFAULTENA_Msk          (1UL << SCB_SHCSR_MEMFAULTENA_Pos)             /*!< SCB SHCSR: MEMFAULTENA Mask */
-
-#define SCB_SHCSR_SVCALLPENDED_Pos         15                                             /*!< SCB SHCSR: SVCALLPENDED Position */
-#define SCB_SHCSR_SVCALLPENDED_Msk         (1UL << SCB_SHCSR_SVCALLPENDED_Pos)            /*!< SCB SHCSR: SVCALLPENDED Mask */
-
-#define SCB_SHCSR_BUSFAULTPENDED_Pos       14                                             /*!< SCB SHCSR: BUSFAULTPENDED Position */
-#define SCB_SHCSR_BUSFAULTPENDED_Msk       (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos)          /*!< SCB SHCSR: BUSFAULTPENDED Mask */
-
-#define SCB_SHCSR_MEMFAULTPENDED_Pos       13                                             /*!< SCB SHCSR: MEMFAULTPENDED Position */
-#define SCB_SHCSR_MEMFAULTPENDED_Msk       (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos)          /*!< SCB SHCSR: MEMFAULTPENDED Mask */
-
-#define SCB_SHCSR_USGFAULTPENDED_Pos       12                                             /*!< SCB SHCSR: USGFAULTPENDED Position */
-#define SCB_SHCSR_USGFAULTPENDED_Msk       (1UL << SCB_SHCSR_USGFAULTPENDED_Pos)          /*!< SCB SHCSR: USGFAULTPENDED Mask */
-
-#define SCB_SHCSR_SYSTICKACT_Pos           11                                             /*!< SCB SHCSR: SYSTICKACT Position */
-#define SCB_SHCSR_SYSTICKACT_Msk           (1UL << SCB_SHCSR_SYSTICKACT_Pos)              /*!< SCB SHCSR: SYSTICKACT Mask */
-
-#define SCB_SHCSR_PENDSVACT_Pos            10                                             /*!< SCB SHCSR: PENDSVACT Position */
-#define SCB_SHCSR_PENDSVACT_Msk            (1UL << SCB_SHCSR_PENDSVACT_Pos)               /*!< SCB SHCSR: PENDSVACT Mask */
-
-#define SCB_SHCSR_MONITORACT_Pos            8                                             /*!< SCB SHCSR: MONITORACT Position */
-#define SCB_SHCSR_MONITORACT_Msk           (1UL << SCB_SHCSR_MONITORACT_Pos)              /*!< SCB SHCSR: MONITORACT Mask */
-
-#define SCB_SHCSR_SVCALLACT_Pos             7                                             /*!< SCB SHCSR: SVCALLACT Position */
-#define SCB_SHCSR_SVCALLACT_Msk            (1UL << SCB_SHCSR_SVCALLACT_Pos)               /*!< SCB SHCSR: SVCALLACT Mask */
-
-#define SCB_SHCSR_USGFAULTACT_Pos           3                                             /*!< SCB SHCSR: USGFAULTACT Position */
-#define SCB_SHCSR_USGFAULTACT_Msk          (1UL << SCB_SHCSR_USGFAULTACT_Pos)             /*!< SCB SHCSR: USGFAULTACT Mask */
-
-#define SCB_SHCSR_BUSFAULTACT_Pos           1                                             /*!< SCB SHCSR: BUSFAULTACT Position */
-#define SCB_SHCSR_BUSFAULTACT_Msk          (1UL << SCB_SHCSR_BUSFAULTACT_Pos)             /*!< SCB SHCSR: BUSFAULTACT Mask */
-
-#define SCB_SHCSR_MEMFAULTACT_Pos           0                                             /*!< SCB SHCSR: MEMFAULTACT Position */
-#define SCB_SHCSR_MEMFAULTACT_Msk          (1UL << SCB_SHCSR_MEMFAULTACT_Pos)             /*!< SCB SHCSR: MEMFAULTACT Mask */
-
-/* SCB Configurable Fault Status Registers Definitions */
-#define SCB_CFSR_USGFAULTSR_Pos            16                                             /*!< SCB CFSR: Usage Fault Status Register Position */
-#define SCB_CFSR_USGFAULTSR_Msk            (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos)          /*!< SCB CFSR: Usage Fault Status Register Mask */
-
-#define SCB_CFSR_BUSFAULTSR_Pos             8                                             /*!< SCB CFSR: Bus Fault Status Register Position */
-#define SCB_CFSR_BUSFAULTSR_Msk            (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos)            /*!< SCB CFSR: Bus Fault Status Register Mask */
-
-#define SCB_CFSR_MEMFAULTSR_Pos             0                                             /*!< SCB CFSR: Memory Manage Fault Status Register Position */
-#define SCB_CFSR_MEMFAULTSR_Msk            (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos)            /*!< SCB CFSR: Memory Manage Fault Status Register Mask */
-
-/* SCB Hard Fault Status Registers Definitions */
-#define SCB_HFSR_DEBUGEVT_Pos              31                                             /*!< SCB HFSR: DEBUGEVT Position */
-#define SCB_HFSR_DEBUGEVT_Msk              (1UL << SCB_HFSR_DEBUGEVT_Pos)                 /*!< SCB HFSR: DEBUGEVT Mask */
-
-#define SCB_HFSR_FORCED_Pos                30                                             /*!< SCB HFSR: FORCED Position */
-#define SCB_HFSR_FORCED_Msk                (1UL << SCB_HFSR_FORCED_Pos)                   /*!< SCB HFSR: FORCED Mask */
-
-#define SCB_HFSR_VECTTBL_Pos                1                                             /*!< SCB HFSR: VECTTBL Position */
-#define SCB_HFSR_VECTTBL_Msk               (1UL << SCB_HFSR_VECTTBL_Pos)                  /*!< SCB HFSR: VECTTBL Mask */
-
-/* SCB Debug Fault Status Register Definitions */
-#define SCB_DFSR_EXTERNAL_Pos               4                                             /*!< SCB DFSR: EXTERNAL Position */
-#define SCB_DFSR_EXTERNAL_Msk              (1UL << SCB_DFSR_EXTERNAL_Pos)                 /*!< SCB DFSR: EXTERNAL Mask */
-
-#define SCB_DFSR_VCATCH_Pos                 3                                             /*!< SCB DFSR: VCATCH Position */
-#define SCB_DFSR_VCATCH_Msk                (1UL << SCB_DFSR_VCATCH_Pos)                   /*!< SCB DFSR: VCATCH Mask */
-
-#define SCB_DFSR_DWTTRAP_Pos                2                                             /*!< SCB DFSR: DWTTRAP Position */
-#define SCB_DFSR_DWTTRAP_Msk               (1UL << SCB_DFSR_DWTTRAP_Pos)                  /*!< SCB DFSR: DWTTRAP Mask */
-
-#define SCB_DFSR_BKPT_Pos                   1                                             /*!< SCB DFSR: BKPT Position */
-#define SCB_DFSR_BKPT_Msk                  (1UL << SCB_DFSR_BKPT_Pos)                     /*!< SCB DFSR: BKPT Mask */
-
-#define SCB_DFSR_HALTED_Pos                 0                                             /*!< SCB DFSR: HALTED Position */
-#define SCB_DFSR_HALTED_Msk                (1UL << SCB_DFSR_HALTED_Pos)                   /*!< SCB DFSR: HALTED Mask */
-
-/*@} end of group CMSIS_SCB */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)
-    \brief      Type definitions for the System Control and ID Register not in the SCB
-  @{
- */
-
-/** \brief  Structure type to access the System Control and ID Register not in the SCB.
- */
-typedef struct
-{
-       uint32_t RESERVED0[1];
-  __I  uint32_t ICTR;                    /*!< Offset: 0x004 (R/ )  Interrupt Controller Type Register      */
-  __IO uint32_t ACTLR;                   /*!< Offset: 0x008 (R/W)  Auxiliary Control Register              */
-} SCnSCB_Type;
-
-/* Interrupt Controller Type Register Definitions */
-#define SCnSCB_ICTR_INTLINESNUM_Pos         0                                          /*!< ICTR: INTLINESNUM Position */
-#define SCnSCB_ICTR_INTLINESNUM_Msk        (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos)      /*!< ICTR: INTLINESNUM Mask */
-
-/* Auxiliary Control Register Definitions */
-#define SCnSCB_ACTLR_DISOOFP_Pos            9                                          /*!< ACTLR: DISOOFP Position */
-#define SCnSCB_ACTLR_DISOOFP_Msk           (1UL << SCnSCB_ACTLR_DISOOFP_Pos)           /*!< ACTLR: DISOOFP Mask */
-
-#define SCnSCB_ACTLR_DISFPCA_Pos            8                                          /*!< ACTLR: DISFPCA Position */
-#define SCnSCB_ACTLR_DISFPCA_Msk           (1UL << SCnSCB_ACTLR_DISFPCA_Pos)           /*!< ACTLR: DISFPCA Mask */
-
-#define SCnSCB_ACTLR_DISFOLD_Pos            2                                          /*!< ACTLR: DISFOLD Position */
-#define SCnSCB_ACTLR_DISFOLD_Msk           (1UL << SCnSCB_ACTLR_DISFOLD_Pos)           /*!< ACTLR: DISFOLD Mask */
-
-#define SCnSCB_ACTLR_DISDEFWBUF_Pos         1                                          /*!< ACTLR: DISDEFWBUF Position */
-#define SCnSCB_ACTLR_DISDEFWBUF_Msk        (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos)        /*!< ACTLR: DISDEFWBUF Mask */
-
-#define SCnSCB_ACTLR_DISMCYCINT_Pos         0                                          /*!< ACTLR: DISMCYCINT Position */
-#define SCnSCB_ACTLR_DISMCYCINT_Msk        (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos)        /*!< ACTLR: DISMCYCINT Mask */
-
-/*@} end of group CMSIS_SCnotSCB */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_SysTick     System Tick Timer (SysTick)
-    \brief      Type definitions for the System Timer Registers.
-  @{
- */
-
-/** \brief  Structure type to access the System Timer (SysTick).
- */
-typedef struct
-{
-  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  SysTick Control and Status Register */
-  __IO uint32_t LOAD;                    /*!< Offset: 0x004 (R/W)  SysTick Reload Value Register       */
-  __IO uint32_t VAL;                     /*!< Offset: 0x008 (R/W)  SysTick Current Value Register      */
-  __I  uint32_t CALIB;                   /*!< Offset: 0x00C (R/ )  SysTick Calibration Register        */
-} SysTick_Type;
-
-/* SysTick Control / Status Register Definitions */
-#define SysTick_CTRL_COUNTFLAG_Pos         16                                             /*!< SysTick CTRL: COUNTFLAG Position */
-#define SysTick_CTRL_COUNTFLAG_Msk         (1UL << SysTick_CTRL_COUNTFLAG_Pos)            /*!< SysTick CTRL: COUNTFLAG Mask */
-
-#define SysTick_CTRL_CLKSOURCE_Pos          2                                             /*!< SysTick CTRL: CLKSOURCE Position */
-#define SysTick_CTRL_CLKSOURCE_Msk         (1UL << SysTick_CTRL_CLKSOURCE_Pos)            /*!< SysTick CTRL: CLKSOURCE Mask */
-
-#define SysTick_CTRL_TICKINT_Pos            1                                             /*!< SysTick CTRL: TICKINT Position */
-#define SysTick_CTRL_TICKINT_Msk           (1UL << SysTick_CTRL_TICKINT_Pos)              /*!< SysTick CTRL: TICKINT Mask */
-
-#define SysTick_CTRL_ENABLE_Pos             0                                             /*!< SysTick CTRL: ENABLE Position */
-#define SysTick_CTRL_ENABLE_Msk            (1UL << SysTick_CTRL_ENABLE_Pos)               /*!< SysTick CTRL: ENABLE Mask */
-
-/* SysTick Reload Register Definitions */
-#define SysTick_LOAD_RELOAD_Pos             0                                             /*!< SysTick LOAD: RELOAD Position */
-#define SysTick_LOAD_RELOAD_Msk            (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos)        /*!< SysTick LOAD: RELOAD Mask */
-
-/* SysTick Current Register Definitions */
-#define SysTick_VAL_CURRENT_Pos             0                                             /*!< SysTick VAL: CURRENT Position */
-#define SysTick_VAL_CURRENT_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick VAL: CURRENT Mask */
-
-/* SysTick Calibration Register Definitions */
-#define SysTick_CALIB_NOREF_Pos            31                                             /*!< SysTick CALIB: NOREF Position */
-#define SysTick_CALIB_NOREF_Msk            (1UL << SysTick_CALIB_NOREF_Pos)               /*!< SysTick CALIB: NOREF Mask */
-
-#define SysTick_CALIB_SKEW_Pos             30                                             /*!< SysTick CALIB: SKEW Position */
-#define SysTick_CALIB_SKEW_Msk             (1UL << SysTick_CALIB_SKEW_Pos)                /*!< SysTick CALIB: SKEW Mask */
-
-#define SysTick_CALIB_TENMS_Pos             0                                             /*!< SysTick CALIB: TENMS Position */
-#define SysTick_CALIB_TENMS_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick CALIB: TENMS Mask */
-
-/*@} end of group CMSIS_SysTick */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_ITM     Instrumentation Trace Macrocell (ITM)
-    \brief      Type definitions for the Instrumentation Trace Macrocell (ITM)
-  @{
- */
-
-/** \brief  Structure type to access the Instrumentation Trace Macrocell Register (ITM).
- */
-typedef struct
-{
-  __O  union
-  {
-    __O  uint8_t    u8;                  /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 8-bit                   */
-    __O  uint16_t   u16;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 16-bit                  */
-    __O  uint32_t   u32;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 32-bit                  */
-  }  PORT [32];                          /*!< Offset: 0x000 ( /W)  ITM Stimulus Port Registers               */
-       uint32_t RESERVED0[864];
-  __IO uint32_t TER;                     /*!< Offset: 0xE00 (R/W)  ITM Trace Enable Register                 */
-       uint32_t RESERVED1[15];
-  __IO uint32_t TPR;                     /*!< Offset: 0xE40 (R/W)  ITM Trace Privilege Register              */
-       uint32_t RESERVED2[15];
-  __IO uint32_t TCR;                     /*!< Offset: 0xE80 (R/W)  ITM Trace Control Register                */
-       uint32_t RESERVED3[29];
-  __O  uint32_t IWR;                     /*!< Offset: 0xEF8 ( /W)  ITM Integration Write Register            */
-  __I  uint32_t IRR;                     /*!< Offset: 0xEFC (R/ )  ITM Integration Read Register             */
-  __IO uint32_t IMCR;                    /*!< Offset: 0xF00 (R/W)  ITM Integration Mode Control Register     */
-       uint32_t RESERVED4[43];
-  __O  uint32_t LAR;                     /*!< Offset: 0xFB0 ( /W)  ITM Lock Access Register                  */
-  __I  uint32_t LSR;                     /*!< Offset: 0xFB4 (R/ )  ITM Lock Status Register                  */
-       uint32_t RESERVED5[6];
-  __I  uint32_t PID4;                    /*!< Offset: 0xFD0 (R/ )  ITM Peripheral Identification Register #4 */
-  __I  uint32_t PID5;                    /*!< Offset: 0xFD4 (R/ )  ITM Peripheral Identification Register #5 */
-  __I  uint32_t PID6;                    /*!< Offset: 0xFD8 (R/ )  ITM Peripheral Identification Register #6 */
-  __I  uint32_t PID7;                    /*!< Offset: 0xFDC (R/ )  ITM Peripheral Identification Register #7 */
-  __I  uint32_t PID0;                    /*!< Offset: 0xFE0 (R/ )  ITM Peripheral Identification Register #0 */
-  __I  uint32_t PID1;                    /*!< Offset: 0xFE4 (R/ )  ITM Peripheral Identification Register #1 */
-  __I  uint32_t PID2;                    /*!< Offset: 0xFE8 (R/ )  ITM Peripheral Identification Register #2 */
-  __I  uint32_t PID3;                    /*!< Offset: 0xFEC (R/ )  ITM Peripheral Identification Register #3 */
-  __I  uint32_t CID0;                    /*!< Offset: 0xFF0 (R/ )  ITM Component  Identification Register #0 */
-  __I  uint32_t CID1;                    /*!< Offset: 0xFF4 (R/ )  ITM Component  Identification Register #1 */
-  __I  uint32_t CID2;                    /*!< Offset: 0xFF8 (R/ )  ITM Component  Identification Register #2 */
-  __I  uint32_t CID3;                    /*!< Offset: 0xFFC (R/ )  ITM Component  Identification Register #3 */
-} ITM_Type;
-
-/* ITM Trace Privilege Register Definitions */
-#define ITM_TPR_PRIVMASK_Pos                0                                             /*!< ITM TPR: PRIVMASK Position */
-#define ITM_TPR_PRIVMASK_Msk               (0xFUL << ITM_TPR_PRIVMASK_Pos)                /*!< ITM TPR: PRIVMASK Mask */
-
-/* ITM Trace Control Register Definitions */
-#define ITM_TCR_BUSY_Pos                   23                                             /*!< ITM TCR: BUSY Position */
-#define ITM_TCR_BUSY_Msk                   (1UL << ITM_TCR_BUSY_Pos)                      /*!< ITM TCR: BUSY Mask */
-
-#define ITM_TCR_TraceBusID_Pos             16                                             /*!< ITM TCR: ATBID Position */
-#define ITM_TCR_TraceBusID_Msk             (0x7FUL << ITM_TCR_TraceBusID_Pos)             /*!< ITM TCR: ATBID Mask */
-
-#define ITM_TCR_GTSFREQ_Pos                10                                             /*!< ITM TCR: Global timestamp frequency Position */
-#define ITM_TCR_GTSFREQ_Msk                (3UL << ITM_TCR_GTSFREQ_Pos)                   /*!< ITM TCR: Global timestamp frequency Mask */
-
-#define ITM_TCR_TSPrescale_Pos              8                                             /*!< ITM TCR: TSPrescale Position */
-#define ITM_TCR_TSPrescale_Msk             (3UL << ITM_TCR_TSPrescale_Pos)                /*!< ITM TCR: TSPrescale Mask */
-
-#define ITM_TCR_SWOENA_Pos                  4                                             /*!< ITM TCR: SWOENA Position */
-#define ITM_TCR_SWOENA_Msk                 (1UL << ITM_TCR_SWOENA_Pos)                    /*!< ITM TCR: SWOENA Mask */
-
-#define ITM_TCR_DWTENA_Pos                  3                                             /*!< ITM TCR: DWTENA Position */
-#define ITM_TCR_DWTENA_Msk                 (1UL << ITM_TCR_DWTENA_Pos)                    /*!< ITM TCR: DWTENA Mask */
-
-#define ITM_TCR_SYNCENA_Pos                 2                                             /*!< ITM TCR: SYNCENA Position */
-#define ITM_TCR_SYNCENA_Msk                (1UL << ITM_TCR_SYNCENA_Pos)                   /*!< ITM TCR: SYNCENA Mask */
-
-#define ITM_TCR_TSENA_Pos                   1                                             /*!< ITM TCR: TSENA Position */
-#define ITM_TCR_TSENA_Msk                  (1UL << ITM_TCR_TSENA_Pos)                     /*!< ITM TCR: TSENA Mask */
-
-#define ITM_TCR_ITMENA_Pos                  0                                             /*!< ITM TCR: ITM Enable bit Position */
-#define ITM_TCR_ITMENA_Msk                 (1UL << ITM_TCR_ITMENA_Pos)                    /*!< ITM TCR: ITM Enable bit Mask */
-
-/* ITM Integration Write Register Definitions */
-#define ITM_IWR_ATVALIDM_Pos                0                                             /*!< ITM IWR: ATVALIDM Position */
-#define ITM_IWR_ATVALIDM_Msk               (1UL << ITM_IWR_ATVALIDM_Pos)                  /*!< ITM IWR: ATVALIDM Mask */
-
-/* ITM Integration Read Register Definitions */
-#define ITM_IRR_ATREADYM_Pos                0                                             /*!< ITM IRR: ATREADYM Position */
-#define ITM_IRR_ATREADYM_Msk               (1UL << ITM_IRR_ATREADYM_Pos)                  /*!< ITM IRR: ATREADYM Mask */
-
-/* ITM Integration Mode Control Register Definitions */
-#define ITM_IMCR_INTEGRATION_Pos            0                                             /*!< ITM IMCR: INTEGRATION Position */
-#define ITM_IMCR_INTEGRATION_Msk           (1UL << ITM_IMCR_INTEGRATION_Pos)              /*!< ITM IMCR: INTEGRATION Mask */
-
-/* ITM Lock Status Register Definitions */
-#define ITM_LSR_ByteAcc_Pos                 2                                             /*!< ITM LSR: ByteAcc Position */
-#define ITM_LSR_ByteAcc_Msk                (1UL << ITM_LSR_ByteAcc_Pos)                   /*!< ITM LSR: ByteAcc Mask */
-
-#define ITM_LSR_Access_Pos                  1                                             /*!< ITM LSR: Access Position */
-#define ITM_LSR_Access_Msk                 (1UL << ITM_LSR_Access_Pos)                    /*!< ITM LSR: Access Mask */
-
-#define ITM_LSR_Present_Pos                 0                                             /*!< ITM LSR: Present Position */
-#define ITM_LSR_Present_Msk                (1UL << ITM_LSR_Present_Pos)                   /*!< ITM LSR: Present Mask */
-
-/*@}*/ /* end of group CMSIS_ITM */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_DWT     Data Watchpoint and Trace (DWT)
-    \brief      Type definitions for the Data Watchpoint and Trace (DWT)
-  @{
- */
-
-/** \brief  Structure type to access the Data Watchpoint and Trace Register (DWT).
- */
-typedef struct
-{
-  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  Control Register                          */
-  __IO uint32_t CYCCNT;                  /*!< Offset: 0x004 (R/W)  Cycle Count Register                      */
-  __IO uint32_t CPICNT;                  /*!< Offset: 0x008 (R/W)  CPI Count Register                        */
-  __IO uint32_t EXCCNT;                  /*!< Offset: 0x00C (R/W)  Exception Overhead Count Register         */
-  __IO uint32_t SLEEPCNT;                /*!< Offset: 0x010 (R/W)  Sleep Count Register                      */
-  __IO uint32_t LSUCNT;                  /*!< Offset: 0x014 (R/W)  LSU Count Register                        */
-  __IO uint32_t FOLDCNT;                 /*!< Offset: 0x018 (R/W)  Folded-instruction Count Register         */
-  __I  uint32_t PCSR;                    /*!< Offset: 0x01C (R/ )  Program Counter Sample Register           */
-  __IO uint32_t COMP0;                   /*!< Offset: 0x020 (R/W)  Comparator Register 0                     */
-  __IO uint32_t MASK0;                   /*!< Offset: 0x024 (R/W)  Mask Register 0                           */
-  __IO uint32_t FUNCTION0;               /*!< Offset: 0x028 (R/W)  Function Register 0                       */
-       uint32_t RESERVED0[1];
-  __IO uint32_t COMP1;                   /*!< Offset: 0x030 (R/W)  Comparator Register 1                     */
-  __IO uint32_t MASK1;                   /*!< Offset: 0x034 (R/W)  Mask Register 1                           */
-  __IO uint32_t FUNCTION1;               /*!< Offset: 0x038 (R/W)  Function Register 1                       */
-       uint32_t RESERVED1[1];
-  __IO uint32_t COMP2;                   /*!< Offset: 0x040 (R/W)  Comparator Register 2                     */
-  __IO uint32_t MASK2;                   /*!< Offset: 0x044 (R/W)  Mask Register 2                           */
-  __IO uint32_t FUNCTION2;               /*!< Offset: 0x048 (R/W)  Function Register 2                       */
-       uint32_t RESERVED2[1];
-  __IO uint32_t COMP3;                   /*!< Offset: 0x050 (R/W)  Comparator Register 3                     */
-  __IO uint32_t MASK3;                   /*!< Offset: 0x054 (R/W)  Mask Register 3                           */
-  __IO uint32_t FUNCTION3;               /*!< Offset: 0x058 (R/W)  Function Register 3                       */
-} DWT_Type;
-
-/* DWT Control Register Definitions */
-#define DWT_CTRL_NUMCOMP_Pos               28                                          /*!< DWT CTRL: NUMCOMP Position */
-#define DWT_CTRL_NUMCOMP_Msk               (0xFUL << DWT_CTRL_NUMCOMP_Pos)             /*!< DWT CTRL: NUMCOMP Mask */
-
-#define DWT_CTRL_NOTRCPKT_Pos              27                                          /*!< DWT CTRL: NOTRCPKT Position */
-#define DWT_CTRL_NOTRCPKT_Msk              (0x1UL << DWT_CTRL_NOTRCPKT_Pos)            /*!< DWT CTRL: NOTRCPKT Mask */
-
-#define DWT_CTRL_NOEXTTRIG_Pos             26                                          /*!< DWT CTRL: NOEXTTRIG Position */
-#define DWT_CTRL_NOEXTTRIG_Msk             (0x1UL << DWT_CTRL_NOEXTTRIG_Pos)           /*!< DWT CTRL: NOEXTTRIG Mask */
-
-#define DWT_CTRL_NOCYCCNT_Pos              25                                          /*!< DWT CTRL: NOCYCCNT Position */
-#define DWT_CTRL_NOCYCCNT_Msk              (0x1UL << DWT_CTRL_NOCYCCNT_Pos)            /*!< DWT CTRL: NOCYCCNT Mask */
-
-#define DWT_CTRL_NOPRFCNT_Pos              24                                          /*!< DWT CTRL: NOPRFCNT Position */
-#define DWT_CTRL_NOPRFCNT_Msk              (0x1UL << DWT_CTRL_NOPRFCNT_Pos)            /*!< DWT CTRL: NOPRFCNT Mask */
-
-#define DWT_CTRL_CYCEVTENA_Pos             22                                          /*!< DWT CTRL: CYCEVTENA Position */
-#define DWT_CTRL_CYCEVTENA_Msk             (0x1UL << DWT_CTRL_CYCEVTENA_Pos)           /*!< DWT CTRL: CYCEVTENA Mask */
-
-#define DWT_CTRL_FOLDEVTENA_Pos            21                                          /*!< DWT CTRL: FOLDEVTENA Position */
-#define DWT_CTRL_FOLDEVTENA_Msk            (0x1UL << DWT_CTRL_FOLDEVTENA_Pos)          /*!< DWT CTRL: FOLDEVTENA Mask */
-
-#define DWT_CTRL_LSUEVTENA_Pos             20                                          /*!< DWT CTRL: LSUEVTENA Position */
-#define DWT_CTRL_LSUEVTENA_Msk             (0x1UL << DWT_CTRL_LSUEVTENA_Pos)           /*!< DWT CTRL: LSUEVTENA Mask */
-
-#define DWT_CTRL_SLEEPEVTENA_Pos           19                                          /*!< DWT CTRL: SLEEPEVTENA Position */
-#define DWT_CTRL_SLEEPEVTENA_Msk           (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos)         /*!< DWT CTRL: SLEEPEVTENA Mask */
-
-#define DWT_CTRL_EXCEVTENA_Pos             18                                          /*!< DWT CTRL: EXCEVTENA Position */
-#define DWT_CTRL_EXCEVTENA_Msk             (0x1UL << DWT_CTRL_EXCEVTENA_Pos)           /*!< DWT CTRL: EXCEVTENA Mask */
-
-#define DWT_CTRL_CPIEVTENA_Pos             17                                          /*!< DWT CTRL: CPIEVTENA Position */
-#define DWT_CTRL_CPIEVTENA_Msk             (0x1UL << DWT_CTRL_CPIEVTENA_Pos)           /*!< DWT CTRL: CPIEVTENA Mask */
-
-#define DWT_CTRL_EXCTRCENA_Pos             16                                          /*!< DWT CTRL: EXCTRCENA Position */
-#define DWT_CTRL_EXCTRCENA_Msk             (0x1UL << DWT_CTRL_EXCTRCENA_Pos)           /*!< DWT CTRL: EXCTRCENA Mask */
-
-#define DWT_CTRL_PCSAMPLENA_Pos            12                                          /*!< DWT CTRL: PCSAMPLENA Position */
-#define DWT_CTRL_PCSAMPLENA_Msk            (0x1UL << DWT_CTRL_PCSAMPLENA_Pos)          /*!< DWT CTRL: PCSAMPLENA Mask */
-
-#define DWT_CTRL_SYNCTAP_Pos               10                                          /*!< DWT CTRL: SYNCTAP Position */
-#define DWT_CTRL_SYNCTAP_Msk               (0x3UL << DWT_CTRL_SYNCTAP_Pos)             /*!< DWT CTRL: SYNCTAP Mask */
-
-#define DWT_CTRL_CYCTAP_Pos                 9                                          /*!< DWT CTRL: CYCTAP Position */
-#define DWT_CTRL_CYCTAP_Msk                (0x1UL << DWT_CTRL_CYCTAP_Pos)              /*!< DWT CTRL: CYCTAP Mask */
-
-#define DWT_CTRL_POSTINIT_Pos               5                                          /*!< DWT CTRL: POSTINIT Position */
-#define DWT_CTRL_POSTINIT_Msk              (0xFUL << DWT_CTRL_POSTINIT_Pos)            /*!< DWT CTRL: POSTINIT Mask */
-
-#define DWT_CTRL_POSTPRESET_Pos             1                                          /*!< DWT CTRL: POSTPRESET Position */
-#define DWT_CTRL_POSTPRESET_Msk            (0xFUL << DWT_CTRL_POSTPRESET_Pos)          /*!< DWT CTRL: POSTPRESET Mask */
-
-#define DWT_CTRL_CYCCNTENA_Pos              0                                          /*!< DWT CTRL: CYCCNTENA Position */
-#define DWT_CTRL_CYCCNTENA_Msk             (0x1UL << DWT_CTRL_CYCCNTENA_Pos)           /*!< DWT CTRL: CYCCNTENA Mask */
-
-/* DWT CPI Count Register Definitions */
-#define DWT_CPICNT_CPICNT_Pos               0                                          /*!< DWT CPICNT: CPICNT Position */
-#define DWT_CPICNT_CPICNT_Msk              (0xFFUL << DWT_CPICNT_CPICNT_Pos)           /*!< DWT CPICNT: CPICNT Mask */
-
-/* DWT Exception Overhead Count Register Definitions */
-#define DWT_EXCCNT_EXCCNT_Pos               0                                          /*!< DWT EXCCNT: EXCCNT Position */
-#define DWT_EXCCNT_EXCCNT_Msk              (0xFFUL << DWT_EXCCNT_EXCCNT_Pos)           /*!< DWT EXCCNT: EXCCNT Mask */
-
-/* DWT Sleep Count Register Definitions */
-#define DWT_SLEEPCNT_SLEEPCNT_Pos           0                                          /*!< DWT SLEEPCNT: SLEEPCNT Position */
-#define DWT_SLEEPCNT_SLEEPCNT_Msk          (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos)       /*!< DWT SLEEPCNT: SLEEPCNT Mask */
-
-/* DWT LSU Count Register Definitions */
-#define DWT_LSUCNT_LSUCNT_Pos               0                                          /*!< DWT LSUCNT: LSUCNT Position */
-#define DWT_LSUCNT_LSUCNT_Msk              (0xFFUL << DWT_LSUCNT_LSUCNT_Pos)           /*!< DWT LSUCNT: LSUCNT Mask */
-
-/* DWT Folded-instruction Count Register Definitions */
-#define DWT_FOLDCNT_FOLDCNT_Pos             0                                          /*!< DWT FOLDCNT: FOLDCNT Position */
-#define DWT_FOLDCNT_FOLDCNT_Msk            (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos)         /*!< DWT FOLDCNT: FOLDCNT Mask */
-
-/* DWT Comparator Mask Register Definitions */
-#define DWT_MASK_MASK_Pos                   0                                          /*!< DWT MASK: MASK Position */
-#define DWT_MASK_MASK_Msk                  (0x1FUL << DWT_MASK_MASK_Pos)               /*!< DWT MASK: MASK Mask */
-
-/* DWT Comparator Function Register Definitions */
-#define DWT_FUNCTION_MATCHED_Pos           24                                          /*!< DWT FUNCTION: MATCHED Position */
-#define DWT_FUNCTION_MATCHED_Msk           (0x1UL << DWT_FUNCTION_MATCHED_Pos)         /*!< DWT FUNCTION: MATCHED Mask */
-
-#define DWT_FUNCTION_DATAVADDR1_Pos        16                                          /*!< DWT FUNCTION: DATAVADDR1 Position */
-#define DWT_FUNCTION_DATAVADDR1_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos)      /*!< DWT FUNCTION: DATAVADDR1 Mask */
-
-#define DWT_FUNCTION_DATAVADDR0_Pos        12                                          /*!< DWT FUNCTION: DATAVADDR0 Position */
-#define DWT_FUNCTION_DATAVADDR0_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos)      /*!< DWT FUNCTION: DATAVADDR0 Mask */
-
-#define DWT_FUNCTION_DATAVSIZE_Pos         10                                          /*!< DWT FUNCTION: DATAVSIZE Position */
-#define DWT_FUNCTION_DATAVSIZE_Msk         (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos)       /*!< DWT FUNCTION: DATAVSIZE Mask */
-
-#define DWT_FUNCTION_LNK1ENA_Pos            9                                          /*!< DWT FUNCTION: LNK1ENA Position */
-#define DWT_FUNCTION_LNK1ENA_Msk           (0x1UL << DWT_FUNCTION_LNK1ENA_Pos)         /*!< DWT FUNCTION: LNK1ENA Mask */
-
-#define DWT_FUNCTION_DATAVMATCH_Pos         8                                          /*!< DWT FUNCTION: DATAVMATCH Position */
-#define DWT_FUNCTION_DATAVMATCH_Msk        (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos)      /*!< DWT FUNCTION: DATAVMATCH Mask */
-
-#define DWT_FUNCTION_CYCMATCH_Pos           7                                          /*!< DWT FUNCTION: CYCMATCH Position */
-#define DWT_FUNCTION_CYCMATCH_Msk          (0x1UL << DWT_FUNCTION_CYCMATCH_Pos)        /*!< DWT FUNCTION: CYCMATCH Mask */
-
-#define DWT_FUNCTION_EMITRANGE_Pos          5                                          /*!< DWT FUNCTION: EMITRANGE Position */
-#define DWT_FUNCTION_EMITRANGE_Msk         (0x1UL << DWT_FUNCTION_EMITRANGE_Pos)       /*!< DWT FUNCTION: EMITRANGE Mask */
-
-#define DWT_FUNCTION_FUNCTION_Pos           0                                          /*!< DWT FUNCTION: FUNCTION Position */
-#define DWT_FUNCTION_FUNCTION_Msk          (0xFUL << DWT_FUNCTION_FUNCTION_Pos)        /*!< DWT FUNCTION: FUNCTION Mask */
-
-/*@}*/ /* end of group CMSIS_DWT */
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_TPI     Trace Port Interface (TPI)
-    \brief      Type definitions for the Trace Port Interface (TPI)
-  @{
- */
-
-/** \brief  Structure type to access the Trace Port Interface Register (TPI).
- */
-typedef struct
-{
-  __IO uint32_t SSPSR;                   /*!< Offset: 0x000 (R/ )  Supported Parallel Port Size Register     */
-  __IO uint32_t CSPSR;                   /*!< Offset: 0x004 (R/W)  Current Parallel Port Size Register */
-       uint32_t RESERVED0[2];
-  __IO uint32_t ACPR;                    /*!< Offset: 0x010 (R/W)  Asynchronous Clock Prescaler Register */
-       uint32_t RESERVED1[55];
-  __IO uint32_t SPPR;                    /*!< Offset: 0x0F0 (R/W)  Selected Pin Protocol Register */
-       uint32_t RESERVED2[131];
-  __I  uint32_t FFSR;                    /*!< Offset: 0x300 (R/ )  Formatter and Flush Status Register */
-  __IO uint32_t FFCR;                    /*!< Offset: 0x304 (R/W)  Formatter and Flush Control Register */
-  __I  uint32_t FSCR;                    /*!< Offset: 0x308 (R/ )  Formatter Synchronization Counter Register */
-       uint32_t RESERVED3[759];
-  __I  uint32_t TRIGGER;                 /*!< Offset: 0xEE8 (R/ )  TRIGGER */
-  __I  uint32_t FIFO0;                   /*!< Offset: 0xEEC (R/ )  Integration ETM Data */
-  __I  uint32_t ITATBCTR2;               /*!< Offset: 0xEF0 (R/ )  ITATBCTR2 */
-       uint32_t RESERVED4[1];
-  __I  uint32_t ITATBCTR0;               /*!< Offset: 0xEF8 (R/ )  ITATBCTR0 */
-  __I  uint32_t FIFO1;                   /*!< Offset: 0xEFC (R/ )  Integration ITM Data */
-  __IO uint32_t ITCTRL;                  /*!< Offset: 0xF00 (R/W)  Integration Mode Control */
-       uint32_t RESERVED5[39];
-  __IO uint32_t CLAIMSET;                /*!< Offset: 0xFA0 (R/W)  Claim tag set */
-  __IO uint32_t CLAIMCLR;                /*!< Offset: 0xFA4 (R/W)  Claim tag clear */
-       uint32_t RESERVED7[8];
-  __I  uint32_t DEVID;                   /*!< Offset: 0xFC8 (R/ )  TPIU_DEVID */
-  __I  uint32_t DEVTYPE;                 /*!< Offset: 0xFCC (R/ )  TPIU_DEVTYPE */
-} TPI_Type;
-
-/* TPI Asynchronous Clock Prescaler Register Definitions */
-#define TPI_ACPR_PRESCALER_Pos              0                                          /*!< TPI ACPR: PRESCALER Position */
-#define TPI_ACPR_PRESCALER_Msk             (0x1FFFUL << TPI_ACPR_PRESCALER_Pos)        /*!< TPI ACPR: PRESCALER Mask */
-
-/* TPI Selected Pin Protocol Register Definitions */
-#define TPI_SPPR_TXMODE_Pos                 0                                          /*!< TPI SPPR: TXMODE Position */
-#define TPI_SPPR_TXMODE_Msk                (0x3UL << TPI_SPPR_TXMODE_Pos)              /*!< TPI SPPR: TXMODE Mask */
-
-/* TPI Formatter and Flush Status Register Definitions */
-#define TPI_FFSR_FtNonStop_Pos              3                                          /*!< TPI FFSR: FtNonStop Position */
-#define TPI_FFSR_FtNonStop_Msk             (0x1UL << TPI_FFSR_FtNonStop_Pos)           /*!< TPI FFSR: FtNonStop Mask */
-
-#define TPI_FFSR_TCPresent_Pos              2                                          /*!< TPI FFSR: TCPresent Position */
-#define TPI_FFSR_TCPresent_Msk             (0x1UL << TPI_FFSR_TCPresent_Pos)           /*!< TPI FFSR: TCPresent Mask */
-
-#define TPI_FFSR_FtStopped_Pos              1                                          /*!< TPI FFSR: FtStopped Position */
-#define TPI_FFSR_FtStopped_Msk             (0x1UL << TPI_FFSR_FtStopped_Pos)           /*!< TPI FFSR: FtStopped Mask */
-
-#define TPI_FFSR_FlInProg_Pos               0                                          /*!< TPI FFSR: FlInProg Position */
-#define TPI_FFSR_FlInProg_Msk              (0x1UL << TPI_FFSR_FlInProg_Pos)            /*!< TPI FFSR: FlInProg Mask */
-
-/* TPI Formatter and Flush Control Register Definitions */
-#define TPI_FFCR_TrigIn_Pos                 8                                          /*!< TPI FFCR: TrigIn Position */
-#define TPI_FFCR_TrigIn_Msk                (0x1UL << TPI_FFCR_TrigIn_Pos)              /*!< TPI FFCR: TrigIn Mask */
-
-#define TPI_FFCR_EnFCont_Pos                1                                          /*!< TPI FFCR: EnFCont Position */
-#define TPI_FFCR_EnFCont_Msk               (0x1UL << TPI_FFCR_EnFCont_Pos)             /*!< TPI FFCR: EnFCont Mask */
-
-/* TPI TRIGGER Register Definitions */
-#define TPI_TRIGGER_TRIGGER_Pos             0                                          /*!< TPI TRIGGER: TRIGGER Position */
-#define TPI_TRIGGER_TRIGGER_Msk            (0x1UL << TPI_TRIGGER_TRIGGER_Pos)          /*!< TPI TRIGGER: TRIGGER Mask */
-
-/* TPI Integration ETM Data Register Definitions (FIFO0) */
-#define TPI_FIFO0_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO0: ITM_ATVALID Position */
-#define TPI_FIFO0_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos)        /*!< TPI FIFO0: ITM_ATVALID Mask */
-
-#define TPI_FIFO0_ITM_bytecount_Pos        27                                          /*!< TPI FIFO0: ITM_bytecount Position */
-#define TPI_FIFO0_ITM_bytecount_Msk        (0x3UL << TPI_FIFO0_ITM_bytecount_Pos)      /*!< TPI FIFO0: ITM_bytecount Mask */
-
-#define TPI_FIFO0_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO0: ETM_ATVALID Position */
-#define TPI_FIFO0_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos)        /*!< TPI FIFO0: ETM_ATVALID Mask */
-
-#define TPI_FIFO0_ETM_bytecount_Pos        24                                          /*!< TPI FIFO0: ETM_bytecount Position */
-#define TPI_FIFO0_ETM_bytecount_Msk        (0x3UL << TPI_FIFO0_ETM_bytecount_Pos)      /*!< TPI FIFO0: ETM_bytecount Mask */
-
-#define TPI_FIFO0_ETM2_Pos                 16                                          /*!< TPI FIFO0: ETM2 Position */
-#define TPI_FIFO0_ETM2_Msk                 (0xFFUL << TPI_FIFO0_ETM2_Pos)              /*!< TPI FIFO0: ETM2 Mask */
-
-#define TPI_FIFO0_ETM1_Pos                  8                                          /*!< TPI FIFO0: ETM1 Position */
-#define TPI_FIFO0_ETM1_Msk                 (0xFFUL << TPI_FIFO0_ETM1_Pos)              /*!< TPI FIFO0: ETM1 Mask */
-
-#define TPI_FIFO0_ETM0_Pos                  0                                          /*!< TPI FIFO0: ETM0 Position */
-#define TPI_FIFO0_ETM0_Msk                 (0xFFUL << TPI_FIFO0_ETM0_Pos)              /*!< TPI FIFO0: ETM0 Mask */
-
-/* TPI ITATBCTR2 Register Definitions */
-#define TPI_ITATBCTR2_ATREADY_Pos           0                                          /*!< TPI ITATBCTR2: ATREADY Position */
-#define TPI_ITATBCTR2_ATREADY_Msk          (0x1UL << TPI_ITATBCTR2_ATREADY_Pos)        /*!< TPI ITATBCTR2: ATREADY Mask */
-
-/* TPI Integration ITM Data Register Definitions (FIFO1) */
-#define TPI_FIFO1_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO1: ITM_ATVALID Position */
-#define TPI_FIFO1_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos)        /*!< TPI FIFO1: ITM_ATVALID Mask */
-
-#define TPI_FIFO1_ITM_bytecount_Pos        27                                          /*!< TPI FIFO1: ITM_bytecount Position */
-#define TPI_FIFO1_ITM_bytecount_Msk        (0x3UL << TPI_FIFO1_ITM_bytecount_Pos)      /*!< TPI FIFO1: ITM_bytecount Mask */
-
-#define TPI_FIFO1_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO1: ETM_ATVALID Position */
-#define TPI_FIFO1_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos)        /*!< TPI FIFO1: ETM_ATVALID Mask */
-
-#define TPI_FIFO1_ETM_bytecount_Pos        24                                          /*!< TPI FIFO1: ETM_bytecount Position */
-#define TPI_FIFO1_ETM_bytecount_Msk        (0x3UL << TPI_FIFO1_ETM_bytecount_Pos)      /*!< TPI FIFO1: ETM_bytecount Mask */
-
-#define TPI_FIFO1_ITM2_Pos                 16                                          /*!< TPI FIFO1: ITM2 Position */
-#define TPI_FIFO1_ITM2_Msk                 (0xFFUL << TPI_FIFO1_ITM2_Pos)              /*!< TPI FIFO1: ITM2 Mask */
-
-#define TPI_FIFO1_ITM1_Pos                  8                                          /*!< TPI FIFO1: ITM1 Position */
-#define TPI_FIFO1_ITM1_Msk                 (0xFFUL << TPI_FIFO1_ITM1_Pos)              /*!< TPI FIFO1: ITM1 Mask */
-
-#define TPI_FIFO1_ITM0_Pos                  0                                          /*!< TPI FIFO1: ITM0 Position */
-#define TPI_FIFO1_ITM0_Msk                 (0xFFUL << TPI_FIFO1_ITM0_Pos)              /*!< TPI FIFO1: ITM0 Mask */
-
-/* TPI ITATBCTR0 Register Definitions */
-#define TPI_ITATBCTR0_ATREADY_Pos           0                                          /*!< TPI ITATBCTR0: ATREADY Position */
-#define TPI_ITATBCTR0_ATREADY_Msk          (0x1UL << TPI_ITATBCTR0_ATREADY_Pos)        /*!< TPI ITATBCTR0: ATREADY Mask */
-
-/* TPI Integration Mode Control Register Definitions */
-#define TPI_ITCTRL_Mode_Pos                 0                                          /*!< TPI ITCTRL: Mode Position */
-#define TPI_ITCTRL_Mode_Msk                (0x1UL << TPI_ITCTRL_Mode_Pos)              /*!< TPI ITCTRL: Mode Mask */
-
-/* TPI DEVID Register Definitions */
-#define TPI_DEVID_NRZVALID_Pos             11                                          /*!< TPI DEVID: NRZVALID Position */
-#define TPI_DEVID_NRZVALID_Msk             (0x1UL << TPI_DEVID_NRZVALID_Pos)           /*!< TPI DEVID: NRZVALID Mask */
-
-#define TPI_DEVID_MANCVALID_Pos            10                                          /*!< TPI DEVID: MANCVALID Position */
-#define TPI_DEVID_MANCVALID_Msk            (0x1UL << TPI_DEVID_MANCVALID_Pos)          /*!< TPI DEVID: MANCVALID Mask */
-
-#define TPI_DEVID_PTINVALID_Pos             9                                          /*!< TPI DEVID: PTINVALID Position */
-#define TPI_DEVID_PTINVALID_Msk            (0x1UL << TPI_DEVID_PTINVALID_Pos)          /*!< TPI DEVID: PTINVALID Mask */
-
-#define TPI_DEVID_MinBufSz_Pos              6                                          /*!< TPI DEVID: MinBufSz Position */
-#define TPI_DEVID_MinBufSz_Msk             (0x7UL << TPI_DEVID_MinBufSz_Pos)           /*!< TPI DEVID: MinBufSz Mask */
-
-#define TPI_DEVID_AsynClkIn_Pos             5                                          /*!< TPI DEVID: AsynClkIn Position */
-#define TPI_DEVID_AsynClkIn_Msk            (0x1UL << TPI_DEVID_AsynClkIn_Pos)          /*!< TPI DEVID: AsynClkIn Mask */
-
-#define TPI_DEVID_NrTraceInput_Pos          0                                          /*!< TPI DEVID: NrTraceInput Position */
-#define TPI_DEVID_NrTraceInput_Msk         (0x1FUL << TPI_DEVID_NrTraceInput_Pos)      /*!< TPI DEVID: NrTraceInput Mask */
-
-/* TPI DEVTYPE Register Definitions */
-#define TPI_DEVTYPE_SubType_Pos             0                                          /*!< TPI DEVTYPE: SubType Position */
-#define TPI_DEVTYPE_SubType_Msk            (0xFUL << TPI_DEVTYPE_SubType_Pos)          /*!< TPI DEVTYPE: SubType Mask */
-
-#define TPI_DEVTYPE_MajorType_Pos           4                                          /*!< TPI DEVTYPE: MajorType Position */
-#define TPI_DEVTYPE_MajorType_Msk          (0xFUL << TPI_DEVTYPE_MajorType_Pos)        /*!< TPI DEVTYPE: MajorType Mask */
-
-/*@}*/ /* end of group CMSIS_TPI */
-
-
-#if (__MPU_PRESENT == 1)
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_MPU     Memory Protection Unit (MPU)
-    \brief      Type definitions for the Memory Protection Unit (MPU)
-  @{
- */
-
-/** \brief  Structure type to access the Memory Protection Unit (MPU).
- */
-typedef struct
-{
-  __I  uint32_t TYPE;                    /*!< Offset: 0x000 (R/ )  MPU Type Register                              */
-  __IO uint32_t CTRL;                    /*!< Offset: 0x004 (R/W)  MPU Control Register                           */
-  __IO uint32_t RNR;                     /*!< Offset: 0x008 (R/W)  MPU Region RNRber Register                     */
-  __IO uint32_t RBAR;                    /*!< Offset: 0x00C (R/W)  MPU Region Base Address Register               */
-  __IO uint32_t RASR;                    /*!< Offset: 0x010 (R/W)  MPU Region Attribute and Size Register         */
-  __IO uint32_t RBAR_A1;                 /*!< Offset: 0x014 (R/W)  MPU Alias 1 Region Base Address Register       */
-  __IO uint32_t RASR_A1;                 /*!< Offset: 0x018 (R/W)  MPU Alias 1 Region Attribute and Size Register */
-  __IO uint32_t RBAR_A2;                 /*!< Offset: 0x01C (R/W)  MPU Alias 2 Region Base Address Register       */
-  __IO uint32_t RASR_A2;                 /*!< Offset: 0x020 (R/W)  MPU Alias 2 Region Attribute and Size Register */
-  __IO uint32_t RBAR_A3;                 /*!< Offset: 0x024 (R/W)  MPU Alias 3 Region Base Address Register       */
-  __IO uint32_t RASR_A3;                 /*!< Offset: 0x028 (R/W)  MPU Alias 3 Region Attribute and Size Register */
-} MPU_Type;
-
-/* MPU Type Register */
-#define MPU_TYPE_IREGION_Pos               16                                             /*!< MPU TYPE: IREGION Position */
-#define MPU_TYPE_IREGION_Msk               (0xFFUL << MPU_TYPE_IREGION_Pos)               /*!< MPU TYPE: IREGION Mask */
-
-#define MPU_TYPE_DREGION_Pos                8                                             /*!< MPU TYPE: DREGION Position */
-#define MPU_TYPE_DREGION_Msk               (0xFFUL << MPU_TYPE_DREGION_Pos)               /*!< MPU TYPE: DREGION Mask */
-
-#define MPU_TYPE_SEPARATE_Pos               0                                             /*!< MPU TYPE: SEPARATE Position */
-#define MPU_TYPE_SEPARATE_Msk              (1UL << MPU_TYPE_SEPARATE_Pos)                 /*!< MPU TYPE: SEPARATE Mask */
-
-/* MPU Control Register */
-#define MPU_CTRL_PRIVDEFENA_Pos             2                                             /*!< MPU CTRL: PRIVDEFENA Position */
-#define MPU_CTRL_PRIVDEFENA_Msk            (1UL << MPU_CTRL_PRIVDEFENA_Pos)               /*!< MPU CTRL: PRIVDEFENA Mask */
-
-#define MPU_CTRL_HFNMIENA_Pos               1                                             /*!< MPU CTRL: HFNMIENA Position */
-#define MPU_CTRL_HFNMIENA_Msk              (1UL << MPU_CTRL_HFNMIENA_Pos)                 /*!< MPU CTRL: HFNMIENA Mask */
-
-#define MPU_CTRL_ENABLE_Pos                 0                                             /*!< MPU CTRL: ENABLE Position */
-#define MPU_CTRL_ENABLE_Msk                (1UL << MPU_CTRL_ENABLE_Pos)                   /*!< MPU CTRL: ENABLE Mask */
-
-/* MPU Region Number Register */
-#define MPU_RNR_REGION_Pos                  0                                             /*!< MPU RNR: REGION Position */
-#define MPU_RNR_REGION_Msk                 (0xFFUL << MPU_RNR_REGION_Pos)                 /*!< MPU RNR: REGION Mask */
-
-/* MPU Region Base Address Register */
-#define MPU_RBAR_ADDR_Pos                   5                                             /*!< MPU RBAR: ADDR Position */
-#define MPU_RBAR_ADDR_Msk                  (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos)             /*!< MPU RBAR: ADDR Mask */
-
-#define MPU_RBAR_VALID_Pos                  4                                             /*!< MPU RBAR: VALID Position */
-#define MPU_RBAR_VALID_Msk                 (1UL << MPU_RBAR_VALID_Pos)                    /*!< MPU RBAR: VALID Mask */
-
-#define MPU_RBAR_REGION_Pos                 0                                             /*!< MPU RBAR: REGION Position */
-#define MPU_RBAR_REGION_Msk                (0xFUL << MPU_RBAR_REGION_Pos)                 /*!< MPU RBAR: REGION Mask */
-
-/* MPU Region Attribute and Size Register */
-#define MPU_RASR_ATTRS_Pos                 16                                             /*!< MPU RASR: MPU Region Attribute field Position */
-#define MPU_RASR_ATTRS_Msk                 (0xFFFFUL << MPU_RASR_ATTRS_Pos)               /*!< MPU RASR: MPU Region Attribute field Mask */
-
-#define MPU_RASR_XN_Pos                    28                                             /*!< MPU RASR: ATTRS.XN Position */
-#define MPU_RASR_XN_Msk                    (1UL << MPU_RASR_XN_Pos)                       /*!< MPU RASR: ATTRS.XN Mask */
-
-#define MPU_RASR_AP_Pos                    24                                             /*!< MPU RASR: ATTRS.AP Position */
-#define MPU_RASR_AP_Msk                    (0x7UL << MPU_RASR_AP_Pos)                     /*!< MPU RASR: ATTRS.AP Mask */
-
-#define MPU_RASR_TEX_Pos                   19                                             /*!< MPU RASR: ATTRS.TEX Position */
-#define MPU_RASR_TEX_Msk                   (0x7UL << MPU_RASR_TEX_Pos)                    /*!< MPU RASR: ATTRS.TEX Mask */
-
-#define MPU_RASR_S_Pos                     18                                             /*!< MPU RASR: ATTRS.S Position */
-#define MPU_RASR_S_Msk                     (1UL << MPU_RASR_S_Pos)                        /*!< MPU RASR: ATTRS.S Mask */
-
-#define MPU_RASR_C_Pos                     17                                             /*!< MPU RASR: ATTRS.C Position */
-#define MPU_RASR_C_Msk                     (1UL << MPU_RASR_C_Pos)                        /*!< MPU RASR: ATTRS.C Mask */
-
-#define MPU_RASR_B_Pos                     16                                             /*!< MPU RASR: ATTRS.B Position */
-#define MPU_RASR_B_Msk                     (1UL << MPU_RASR_B_Pos)                        /*!< MPU RASR: ATTRS.B Mask */
-
-#define MPU_RASR_SRD_Pos                    8                                             /*!< MPU RASR: Sub-Region Disable Position */
-#define MPU_RASR_SRD_Msk                   (0xFFUL << MPU_RASR_SRD_Pos)                   /*!< MPU RASR: Sub-Region Disable Mask */
-
-#define MPU_RASR_SIZE_Pos                   1                                             /*!< MPU RASR: Region Size Field Position */
-#define MPU_RASR_SIZE_Msk                  (0x1FUL << MPU_RASR_SIZE_Pos)                  /*!< MPU RASR: Region Size Field Mask */
-
-#define MPU_RASR_ENABLE_Pos                 0                                             /*!< MPU RASR: Region enable bit Position */
-#define MPU_RASR_ENABLE_Msk                (1UL << MPU_RASR_ENABLE_Pos)                   /*!< MPU RASR: Region enable bit Disable Mask */
-
-/*@} end of group CMSIS_MPU */
-#endif
-
-
-#if (__FPU_PRESENT == 1)
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_FPU     Floating Point Unit (FPU)
-    \brief      Type definitions for the Floating Point Unit (FPU)
-  @{
- */
-
-/** \brief  Structure type to access the Floating Point Unit (FPU).
- */
-typedef struct
-{
-       uint32_t RESERVED0[1];
-  __IO uint32_t FPCCR;                   /*!< Offset: 0x004 (R/W)  Floating-Point Context Control Register               */
-  __IO uint32_t FPCAR;                   /*!< Offset: 0x008 (R/W)  Floating-Point Context Address Register               */
-  __IO uint32_t FPDSCR;                  /*!< Offset: 0x00C (R/W)  Floating-Point Default Status Control Register        */
-  __I  uint32_t MVFR0;                   /*!< Offset: 0x010 (R/ )  Media and FP Feature Register 0                       */
-  __I  uint32_t MVFR1;                   /*!< Offset: 0x014 (R/ )  Media and FP Feature Register 1                       */
-} FPU_Type;
-
-/* Floating-Point Context Control Register */
-#define FPU_FPCCR_ASPEN_Pos                31                                             /*!< FPCCR: ASPEN bit Position */
-#define FPU_FPCCR_ASPEN_Msk                (1UL << FPU_FPCCR_ASPEN_Pos)                   /*!< FPCCR: ASPEN bit Mask */
-
-#define FPU_FPCCR_LSPEN_Pos                30                                             /*!< FPCCR: LSPEN Position */
-#define FPU_FPCCR_LSPEN_Msk                (1UL << FPU_FPCCR_LSPEN_Pos)                   /*!< FPCCR: LSPEN bit Mask */
-
-#define FPU_FPCCR_MONRDY_Pos                8                                             /*!< FPCCR: MONRDY Position */
-#define FPU_FPCCR_MONRDY_Msk               (1UL << FPU_FPCCR_MONRDY_Pos)                  /*!< FPCCR: MONRDY bit Mask */
-
-#define FPU_FPCCR_BFRDY_Pos                 6                                             /*!< FPCCR: BFRDY Position */
-#define FPU_FPCCR_BFRDY_Msk                (1UL << FPU_FPCCR_BFRDY_Pos)                   /*!< FPCCR: BFRDY bit Mask */
-
-#define FPU_FPCCR_MMRDY_Pos                 5                                             /*!< FPCCR: MMRDY Position */
-#define FPU_FPCCR_MMRDY_Msk                (1UL << FPU_FPCCR_MMRDY_Pos)                   /*!< FPCCR: MMRDY bit Mask */
-
-#define FPU_FPCCR_HFRDY_Pos                 4                                             /*!< FPCCR: HFRDY Position */
-#define FPU_FPCCR_HFRDY_Msk                (1UL << FPU_FPCCR_HFRDY_Pos)                   /*!< FPCCR: HFRDY bit Mask */
-
-#define FPU_FPCCR_THREAD_Pos                3                                             /*!< FPCCR: processor mode bit Position */
-#define FPU_FPCCR_THREAD_Msk               (1UL << FPU_FPCCR_THREAD_Pos)                  /*!< FPCCR: processor mode active bit Mask */
-
-#define FPU_FPCCR_USER_Pos                  1                                             /*!< FPCCR: privilege level bit Position */
-#define FPU_FPCCR_USER_Msk                 (1UL << FPU_FPCCR_USER_Pos)                    /*!< FPCCR: privilege level bit Mask */
-
-#define FPU_FPCCR_LSPACT_Pos                0                                             /*!< FPCCR: Lazy state preservation active bit Position */
-#define FPU_FPCCR_LSPACT_Msk               (1UL << FPU_FPCCR_LSPACT_Pos)                  /*!< FPCCR: Lazy state preservation active bit Mask */
-
-/* Floating-Point Context Address Register */
-#define FPU_FPCAR_ADDRESS_Pos               3                                             /*!< FPCAR: ADDRESS bit Position */
-#define FPU_FPCAR_ADDRESS_Msk              (0x1FFFFFFFUL << FPU_FPCAR_ADDRESS_Pos)        /*!< FPCAR: ADDRESS bit Mask */
-
-/* Floating-Point Default Status Control Register */
-#define FPU_FPDSCR_AHP_Pos                 26                                             /*!< FPDSCR: AHP bit Position */
-#define FPU_FPDSCR_AHP_Msk                 (1UL << FPU_FPDSCR_AHP_Pos)                    /*!< FPDSCR: AHP bit Mask */
-
-#define FPU_FPDSCR_DN_Pos                  25                                             /*!< FPDSCR: DN bit Position */
-#define FPU_FPDSCR_DN_Msk                  (1UL << FPU_FPDSCR_DN_Pos)                     /*!< FPDSCR: DN bit Mask */
-
-#define FPU_FPDSCR_FZ_Pos                  24                                             /*!< FPDSCR: FZ bit Position */
-#define FPU_FPDSCR_FZ_Msk                  (1UL << FPU_FPDSCR_FZ_Pos)                     /*!< FPDSCR: FZ bit Mask */
-
-#define FPU_FPDSCR_RMode_Pos               22                                             /*!< FPDSCR: RMode bit Position */
-#define FPU_FPDSCR_RMode_Msk               (3UL << FPU_FPDSCR_RMode_Pos)                  /*!< FPDSCR: RMode bit Mask */
-
-/* Media and FP Feature Register 0 */
-#define FPU_MVFR0_FP_rounding_modes_Pos    28                                             /*!< MVFR0: FP rounding modes bits Position */
-#define FPU_MVFR0_FP_rounding_modes_Msk    (0xFUL << FPU_MVFR0_FP_rounding_modes_Pos)     /*!< MVFR0: FP rounding modes bits Mask */
-
-#define FPU_MVFR0_Short_vectors_Pos        24                                             /*!< MVFR0: Short vectors bits Position */
-#define FPU_MVFR0_Short_vectors_Msk        (0xFUL << FPU_MVFR0_Short_vectors_Pos)         /*!< MVFR0: Short vectors bits Mask */
-
-#define FPU_MVFR0_Square_root_Pos          20                                             /*!< MVFR0: Square root bits Position */
-#define FPU_MVFR0_Square_root_Msk          (0xFUL << FPU_MVFR0_Square_root_Pos)           /*!< MVFR0: Square root bits Mask */
-
-#define FPU_MVFR0_Divide_Pos               16                                             /*!< MVFR0: Divide bits Position */
-#define FPU_MVFR0_Divide_Msk               (0xFUL << FPU_MVFR0_Divide_Pos)                /*!< MVFR0: Divide bits Mask */
-
-#define FPU_MVFR0_FP_excep_trapping_Pos    12                                             /*!< MVFR0: FP exception trapping bits Position */
-#define FPU_MVFR0_FP_excep_trapping_Msk    (0xFUL << FPU_MVFR0_FP_excep_trapping_Pos)     /*!< MVFR0: FP exception trapping bits Mask */
-
-#define FPU_MVFR0_Double_precision_Pos      8                                             /*!< MVFR0: Double-precision bits Position */
-#define FPU_MVFR0_Double_precision_Msk     (0xFUL << FPU_MVFR0_Double_precision_Pos)      /*!< MVFR0: Double-precision bits Mask */
-
-#define FPU_MVFR0_Single_precision_Pos      4                                             /*!< MVFR0: Single-precision bits Position */
-#define FPU_MVFR0_Single_precision_Msk     (0xFUL << FPU_MVFR0_Single_precision_Pos)      /*!< MVFR0: Single-precision bits Mask */
-
-#define FPU_MVFR0_A_SIMD_registers_Pos      0                                             /*!< MVFR0: A_SIMD registers bits Position */
-#define FPU_MVFR0_A_SIMD_registers_Msk     (0xFUL << FPU_MVFR0_A_SIMD_registers_Pos)      /*!< MVFR0: A_SIMD registers bits Mask */
-
-/* Media and FP Feature Register 1 */
-#define FPU_MVFR1_FP_fused_MAC_Pos         28                                             /*!< MVFR1: FP fused MAC bits Position */
-#define FPU_MVFR1_FP_fused_MAC_Msk         (0xFUL << FPU_MVFR1_FP_fused_MAC_Pos)          /*!< MVFR1: FP fused MAC bits Mask */
-
-#define FPU_MVFR1_FP_HPFP_Pos              24                                             /*!< MVFR1: FP HPFP bits Position */
-#define FPU_MVFR1_FP_HPFP_Msk              (0xFUL << FPU_MVFR1_FP_HPFP_Pos)               /*!< MVFR1: FP HPFP bits Mask */
-
-#define FPU_MVFR1_D_NaN_mode_Pos            4                                             /*!< MVFR1: D_NaN mode bits Position */
-#define FPU_MVFR1_D_NaN_mode_Msk           (0xFUL << FPU_MVFR1_D_NaN_mode_Pos)            /*!< MVFR1: D_NaN mode bits Mask */
-
-#define FPU_MVFR1_FtZ_mode_Pos              0                                             /*!< MVFR1: FtZ mode bits Position */
-#define FPU_MVFR1_FtZ_mode_Msk             (0xFUL << FPU_MVFR1_FtZ_mode_Pos)              /*!< MVFR1: FtZ mode bits Mask */
-
-/*@} end of group CMSIS_FPU */
-#endif
-
-
-/** \ingroup  CMSIS_core_register
-    \defgroup CMSIS_CoreDebug       Core Debug Registers (CoreDebug)
-    \brief      Type definitions for the Core Debug Registers
-  @{
- */
-
-/** \brief  Structure type to access the Core Debug Register (CoreDebug).
- */
-typedef struct
-{
-  __IO uint32_t DHCSR;                   /*!< Offset: 0x000 (R/W)  Debug Halting Control and Status Register    */
-  __O  uint32_t DCRSR;                   /*!< Offset: 0x004 ( /W)  Debug Core Register Selector Register        */
-  __IO uint32_t DCRDR;                   /*!< Offset: 0x008 (R/W)  Debug Core Register Data Register            */
-  __IO uint32_t DEMCR;                   /*!< Offset: 0x00C (R/W)  Debug Exception and Monitor Control Register */
-} CoreDebug_Type;
-
-/* Debug Halting Control and Status Register */
-#define CoreDebug_DHCSR_DBGKEY_Pos         16                                             /*!< CoreDebug DHCSR: DBGKEY Position */
-#define CoreDebug_DHCSR_DBGKEY_Msk         (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos)       /*!< CoreDebug DHCSR: DBGKEY Mask */
-
-#define CoreDebug_DHCSR_S_RESET_ST_Pos     25                                             /*!< CoreDebug DHCSR: S_RESET_ST Position */
-#define CoreDebug_DHCSR_S_RESET_ST_Msk     (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos)        /*!< CoreDebug DHCSR: S_RESET_ST Mask */
-
-#define CoreDebug_DHCSR_S_RETIRE_ST_Pos    24                                             /*!< CoreDebug DHCSR: S_RETIRE_ST Position */
-#define CoreDebug_DHCSR_S_RETIRE_ST_Msk    (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos)       /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */
-
-#define CoreDebug_DHCSR_S_LOCKUP_Pos       19                                             /*!< CoreDebug DHCSR: S_LOCKUP Position */
-#define CoreDebug_DHCSR_S_LOCKUP_Msk       (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos)          /*!< CoreDebug DHCSR: S_LOCKUP Mask */
-
-#define CoreDebug_DHCSR_S_SLEEP_Pos        18                                             /*!< CoreDebug DHCSR: S_SLEEP Position */
-#define CoreDebug_DHCSR_S_SLEEP_Msk        (1UL << CoreDebug_DHCSR_S_SLEEP_Pos)           /*!< CoreDebug DHCSR: S_SLEEP Mask */
-
-#define CoreDebug_DHCSR_S_HALT_Pos         17                                             /*!< CoreDebug DHCSR: S_HALT Position */
-#define CoreDebug_DHCSR_S_HALT_Msk         (1UL << CoreDebug_DHCSR_S_HALT_Pos)            /*!< CoreDebug DHCSR: S_HALT Mask */
-
-#define CoreDebug_DHCSR_S_REGRDY_Pos       16                                             /*!< CoreDebug DHCSR: S_REGRDY Position */
-#define CoreDebug_DHCSR_S_REGRDY_Msk       (1UL << CoreDebug_DHCSR_S_REGRDY_Pos)          /*!< CoreDebug DHCSR: S_REGRDY Mask */
-
-#define CoreDebug_DHCSR_C_SNAPSTALL_Pos     5                                             /*!< CoreDebug DHCSR: C_SNAPSTALL Position */
-#define CoreDebug_DHCSR_C_SNAPSTALL_Msk    (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos)       /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */
-
-#define CoreDebug_DHCSR_C_MASKINTS_Pos      3                                             /*!< CoreDebug DHCSR: C_MASKINTS Position */
-#define CoreDebug_DHCSR_C_MASKINTS_Msk     (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos)        /*!< CoreDebug DHCSR: C_MASKINTS Mask */
-
-#define CoreDebug_DHCSR_C_STEP_Pos          2                                             /*!< CoreDebug DHCSR: C_STEP Position */
-#define CoreDebug_DHCSR_C_STEP_Msk         (1UL << CoreDebug_DHCSR_C_STEP_Pos)            /*!< CoreDebug DHCSR: C_STEP Mask */
-
-#define CoreDebug_DHCSR_C_HALT_Pos          1                                             /*!< CoreDebug DHCSR: C_HALT Position */
-#define CoreDebug_DHCSR_C_HALT_Msk         (1UL << CoreDebug_DHCSR_C_HALT_Pos)            /*!< CoreDebug DHCSR: C_HALT Mask */
-
-#define CoreDebug_DHCSR_C_DEBUGEN_Pos       0                                             /*!< CoreDebug DHCSR: C_DEBUGEN Position */
-#define CoreDebug_DHCSR_C_DEBUGEN_Msk      (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos)         /*!< CoreDebug DHCSR: C_DEBUGEN Mask */
-
-/* Debug Core Register Selector Register */
-#define CoreDebug_DCRSR_REGWnR_Pos         16                                             /*!< CoreDebug DCRSR: REGWnR Position */
-#define CoreDebug_DCRSR_REGWnR_Msk         (1UL << CoreDebug_DCRSR_REGWnR_Pos)            /*!< CoreDebug DCRSR: REGWnR Mask */
-
-#define CoreDebug_DCRSR_REGSEL_Pos          0                                             /*!< CoreDebug DCRSR: REGSEL Position */
-#define CoreDebug_DCRSR_REGSEL_Msk         (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos)         /*!< CoreDebug DCRSR: REGSEL Mask */
-
-/* Debug Exception and Monitor Control Register */
-#define CoreDebug_DEMCR_TRCENA_Pos         24                                             /*!< CoreDebug DEMCR: TRCENA Position */
-#define CoreDebug_DEMCR_TRCENA_Msk         (1UL << CoreDebug_DEMCR_TRCENA_Pos)            /*!< CoreDebug DEMCR: TRCENA Mask */
-
-#define CoreDebug_DEMCR_MON_REQ_Pos        19                                             /*!< CoreDebug DEMCR: MON_REQ Position */
-#define CoreDebug_DEMCR_MON_REQ_Msk        (1UL << CoreDebug_DEMCR_MON_REQ_Pos)           /*!< CoreDebug DEMCR: MON_REQ Mask */
-
-#define CoreDebug_DEMCR_MON_STEP_Pos       18                                             /*!< CoreDebug DEMCR: MON_STEP Position */
-#define CoreDebug_DEMCR_MON_STEP_Msk       (1UL << CoreDebug_DEMCR_MON_STEP_Pos)          /*!< CoreDebug DEMCR: MON_STEP Mask */
-
-#define CoreDebug_DEMCR_MON_PEND_Pos       17                                             /*!< CoreDebug DEMCR: MON_PEND Position */
-#define CoreDebug_DEMCR_MON_PEND_Msk       (1UL << CoreDebug_DEMCR_MON_PEND_Pos)          /*!< CoreDebug DEMCR: MON_PEND Mask */
-
-#define CoreDebug_DEMCR_MON_EN_Pos         16                                             /*!< CoreDebug DEMCR: MON_EN Position */
-#define CoreDebug_DEMCR_MON_EN_Msk         (1UL << CoreDebug_DEMCR_MON_EN_Pos)            /*!< CoreDebug DEMCR: MON_EN Mask */
-
-#define CoreDebug_DEMCR_VC_HARDERR_Pos     10                                             /*!< CoreDebug DEMCR: VC_HARDERR Position */
-#define CoreDebug_DEMCR_VC_HARDERR_Msk     (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos)        /*!< CoreDebug DEMCR: VC_HARDERR Mask */
-
-#define CoreDebug_DEMCR_VC_INTERR_Pos       9                                             /*!< CoreDebug DEMCR: VC_INTERR Position */
-#define CoreDebug_DEMCR_VC_INTERR_Msk      (1UL << CoreDebug_DEMCR_VC_INTERR_Pos)         /*!< CoreDebug DEMCR: VC_INTERR Mask */
-
-#define CoreDebug_DEMCR_VC_BUSERR_Pos       8                                             /*!< CoreDebug DEMCR: VC_BUSERR Position */
-#define CoreDebug_DEMCR_VC_BUSERR_Msk      (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos)         /*!< CoreDebug DEMCR: VC_BUSERR Mask */
-
-#define CoreDebug_DEMCR_VC_STATERR_Pos      7                                             /*!< CoreDebug DEMCR: VC_STATERR Position */
-#define CoreDebug_DEMCR_VC_STATERR_Msk     (1UL << CoreDebug_DEMCR_VC_STATERR_Pos)        /*!< CoreDebug DEMCR: VC_STATERR Mask */
-
-#define CoreDebug_DEMCR_VC_CHKERR_Pos       6                                             /*!< CoreDebug DEMCR: VC_CHKERR Position */
-#define CoreDebug_DEMCR_VC_CHKERR_Msk      (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos)         /*!< CoreDebug DEMCR: VC_CHKERR Mask */
-
-#define CoreDebug_DEMCR_VC_NOCPERR_Pos      5                                             /*!< CoreDebug DEMCR: VC_NOCPERR Position */
-#define CoreDebug_DEMCR_VC_NOCPERR_Msk     (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos)        /*!< CoreDebug DEMCR: VC_NOCPERR Mask */
-
-#define CoreDebug_DEMCR_VC_MMERR_Pos        4                                             /*!< CoreDebug DEMCR: VC_MMERR Position */
-#define CoreDebug_DEMCR_VC_MMERR_Msk       (1UL << CoreDebug_DEMCR_VC_MMERR_Pos)          /*!< CoreDebug DEMCR: VC_MMERR Mask */
-
-#define CoreDebug_DEMCR_VC_CORERESET_Pos    0                                             /*!< CoreDebug DEMCR: VC_CORERESET Position */
-#define CoreDebug_DEMCR_VC_CORERESET_Msk   (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos)      /*!< CoreDebug DEMCR: VC_CORERESET Mask */
-
-/*@} end of group CMSIS_CoreDebug */
-
-
-/** \ingroup    CMSIS_core_register
-    \defgroup   CMSIS_core_base     Core Definitions
-    \brief      Definitions for base addresses, unions, and structures.
-  @{
- */
-
-/* Memory mapping of Cortex-M4 Hardware */
-#define SCS_BASE            (0xE000E000UL)                            /*!< System Control Space Base Address  */
-#define ITM_BASE            (0xE0000000UL)                            /*!< ITM Base Address                   */
-#define DWT_BASE            (0xE0001000UL)                            /*!< DWT Base Address                   */
-#define TPI_BASE            (0xE0040000UL)                            /*!< TPI Base Address                   */
-#define CoreDebug_BASE      (0xE000EDF0UL)                            /*!< Core Debug Base Address            */
-#define SysTick_BASE        (SCS_BASE +  0x0010UL)                    /*!< SysTick Base Address               */
-#define NVIC_BASE           (SCS_BASE +  0x0100UL)                    /*!< NVIC Base Address                  */
-#define SCB_BASE            (SCS_BASE +  0x0D00UL)                    /*!< System Control Block Base Address  */
-
-#define SCnSCB              ((SCnSCB_Type    *)     SCS_BASE      )   /*!< System control Register not in SCB */
-#define SCB                 ((SCB_Type       *)     SCB_BASE      )   /*!< SCB configuration struct           */
-#define SysTick             ((SysTick_Type   *)     SysTick_BASE  )   /*!< SysTick configuration struct       */
-#define NVIC                ((NVIC_Type      *)     NVIC_BASE     )   /*!< NVIC configuration struct          */
-#define ITM                 ((ITM_Type       *)     ITM_BASE      )   /*!< ITM configuration struct           */
-#define DWT                 ((DWT_Type       *)     DWT_BASE      )   /*!< DWT configuration struct           */
-#define TPI                 ((TPI_Type       *)     TPI_BASE      )   /*!< TPI configuration struct           */
-#define CoreDebug           ((CoreDebug_Type *)     CoreDebug_BASE)   /*!< Core Debug configuration struct    */
-
-#if (__MPU_PRESENT == 1)
-  #define MPU_BASE          (SCS_BASE +  0x0D90UL)                    /*!< Memory Protection Unit             */
-  #define MPU               ((MPU_Type       *)     MPU_BASE      )   /*!< Memory Protection Unit             */
-#endif
-
-#if (__FPU_PRESENT == 1)
-  #define FPU_BASE          (SCS_BASE +  0x0F30UL)                    /*!< Floating Point Unit                */
-  #define FPU               ((FPU_Type       *)     FPU_BASE      )   /*!< Floating Point Unit                */
-#endif
-
-/*@} */
-
-
-
-/*******************************************************************************
- *                Hardware Abstraction Layer
-  Core Function Interface contains:
-  - Core NVIC Functions
-  - Core SysTick Functions
-  - Core Debug Functions
-  - Core Register Access Functions
- ******************************************************************************/
-/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
-*/
-
-
-
-/* ##########################   NVIC functions  #################################### */
-/** \ingroup  CMSIS_Core_FunctionInterface
-    \defgroup CMSIS_Core_NVICFunctions NVIC Functions
-    \brief      Functions that manage interrupts and exceptions via the NVIC.
-    @{
- */
-
-/** \brief  Set Priority Grouping
-
-  The function sets the priority grouping field using the required unlock sequence.
-  The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.
-  Only values from 0..7 are used.
-  In case of a conflict between priority grouping and available
-  priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
-
-    \param [in]      PriorityGroup  Priority grouping field.
- */
-__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
-{
-  uint32_t reg_value;
-  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07);               /* only values 0..7 are used          */
-
-  reg_value  =  SCB->AIRCR;                                                   /* read old register configuration    */
-  reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk);             /* clear bits to change               */
-  reg_value  =  (reg_value                                 |
-                ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |
-                (PriorityGroupTmp << 8));                                     /* Insert write key and priorty group */
-  SCB->AIRCR =  reg_value;
-}
-
-
-/** \brief  Get Priority Grouping
-
-  The function reads the priority grouping field from the NVIC Interrupt Controller.
-
-    \return                Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).
- */
-__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
-{
-  return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos);   /* read priority grouping field */
-}
-
-
-/** \brief  Enable External Interrupt
-
-    The function enables a device-specific interrupt in the NVIC interrupt controller.
-
-    \param [in]      IRQn  External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
-{
-/*  NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F));  enable interrupt */
-  NVIC->ISER[(uint32_t)((int32_t)IRQn) >> 5] = (uint32_t)(1 << ((uint32_t)((int32_t)IRQn) & (uint32_t)0x1F)); /* enable interrupt */
-}
-
-
-/** \brief  Disable External Interrupt
-
-    The function disables a device-specific interrupt in the NVIC interrupt controller.
-
-    \param [in]      IRQn  External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
-{
-  NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */
-}
-
-
-/** \brief  Get Pending Interrupt
-
-    The function reads the pending register in the NVIC and returns the pending bit
-    for the specified interrupt.
-
-    \param [in]      IRQn  Interrupt number.
-
-    \return             0  Interrupt status is not pending.
-    \return             1  Interrupt status is pending.
- */
-__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
-{
-  return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */
-}
-
-
-/** \brief  Set Pending Interrupt
-
-    The function sets the pending bit of an external interrupt.
-
-    \param [in]      IRQn  Interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
-{
-  NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */
-}
-
-
-/** \brief  Clear Pending Interrupt
-
-    The function clears the pending bit of an external interrupt.
-
-    \param [in]      IRQn  External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
-{
-  NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
-}
-
-
-/** \brief  Get Active Interrupt
-
-    The function reads the active register in NVIC and returns the active bit.
-
-    \param [in]      IRQn  Interrupt number.
-
-    \return             0  Interrupt status is not active.
-    \return             1  Interrupt status is active.
- */
-__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
-{
-  return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */
-}
-
-
-/** \brief  Set Interrupt Priority
-
-    The function sets the priority of an interrupt.
-
-    \note The priority cannot be set for every core interrupt.
-
-    \param [in]      IRQn  Interrupt number.
-    \param [in]  priority  Priority to set.
- */
-__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
-{
-  if(IRQn < 0) {
-    SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M  System Interrupts */
-  else {
-    NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff);    }        /* set Priority for device specific Interrupts  */
-}
-
-
-/** \brief  Get Interrupt Priority
-
-    The function reads the priority of an interrupt. The interrupt
-    number can be positive to specify an external (device specific)
-    interrupt, or negative to specify an internal (core) interrupt.
-
-
-    \param [in]   IRQn  Interrupt number.
-    \return             Interrupt Priority. Value is aligned automatically to the implemented
-                        priority bits of the microcontroller.
- */
-__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
-{
-
-  if(IRQn < 0) {
-    return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for Cortex-M  system interrupts */
-  else {
-    return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)]           >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for device specific interrupts  */
-}
-
-
-/** \brief  Encode Priority
-
-    The function encodes the priority for an interrupt with the given priority group,
-    preemptive priority value, and subpriority value.
-    In case of a conflict between priority grouping and available
-    priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.
-
-    \param [in]     PriorityGroup  Used priority group.
-    \param [in]   PreemptPriority  Preemptive priority value (starting from 0).
-    \param [in]       SubPriority  Subpriority value (starting from 0).
-    \return                        Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
- */
-__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
-{
-  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */
-  uint32_t PreemptPriorityBits;
-  uint32_t SubPriorityBits;
-
-  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
-  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
-
-  return (
-           ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |
-           ((SubPriority     & ((1 << (SubPriorityBits    )) - 1)))
-         );
-}
-
-
-/** \brief  Decode Priority
-
-    The function decodes an interrupt priority value with a given priority group to
-    preemptive priority value and subpriority value.
-    In case of a conflict between priority grouping and available
-    priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.
-
-    \param [in]         Priority   Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
-    \param [in]     PriorityGroup  Used priority group.
-    \param [out] pPreemptPriority  Preemptive priority value (starting from 0).
-    \param [out]     pSubPriority  Subpriority value (starting from 0).
- */
-__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)
-{
-  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */
-  uint32_t PreemptPriorityBits;
-  uint32_t SubPriorityBits;
-
-  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
-  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
-
-  *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);
-  *pSubPriority     = (Priority                   ) & ((1 << (SubPriorityBits    )) - 1);
-}
-
-
-/** \brief  System Reset
-
-    The function initiates a system reset request to reset the MCU.
- */
-__STATIC_INLINE void NVIC_SystemReset(void)
-{
-  __DSB();                                                     /* Ensure all outstanding memory accesses included
-                                                                  buffered write are completed before reset */
-  SCB->AIRCR  = ((0x5FA << SCB_AIRCR_VECTKEY_Pos)      |
-                 (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
-                 SCB_AIRCR_SYSRESETREQ_Msk);                   /* Keep priority group unchanged */
-  __DSB();                                                     /* Ensure completion of memory access */
-  while(1);                                                    /* wait until reset */
-}
-
-/*@} end of CMSIS_Core_NVICFunctions */
-
-
-
-/* ##################################    SysTick function  ############################################ */
-/** \ingroup  CMSIS_Core_FunctionInterface
-    \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
-    \brief      Functions that configure the System.
-  @{
- */
-
-#if (__Vendor_SysTickConfig == 0)
-
-/** \brief  System Tick Configuration
-
-    The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
-    Counter is in free running mode to generate periodic interrupts.
-
-    \param [in]  ticks  Number of ticks between two interrupts.
-
-    \return          0  Function succeeded.
-    \return          1  Function failed.
-
-    \note     When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
-    function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
-    must contain a vendor-specific implementation of this function.
-
- */
-__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
-{
-  if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk)  return (1);      /* Reload value impossible */
-
-  SysTick->LOAD  = ticks - 1;                                  /* set reload register */
-  NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1);  /* set Priority for Systick Interrupt */
-  SysTick->VAL   = 0;                                          /* Load the SysTick Counter Value */
-  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |
-                   SysTick_CTRL_TICKINT_Msk   |
-                   SysTick_CTRL_ENABLE_Msk;                    /* Enable SysTick IRQ and SysTick Timer */
-  return (0);                                                  /* Function successful */
-}
-
-#endif
-
-/*@} end of CMSIS_Core_SysTickFunctions */
-
-
-
-/* ##################################### Debug In/Output function ########################################### */
-/** \ingroup  CMSIS_Core_FunctionInterface
-    \defgroup CMSIS_core_DebugFunctions ITM Functions
-    \brief   Functions that access the ITM debug interface.
-  @{
- */
-
-extern volatile int32_t ITM_RxBuffer;                    /*!< External variable to receive characters.                         */
-#define                 ITM_RXBUFFER_EMPTY    0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */
-
-
-/** \brief  ITM Send Character
-
-    The function transmits a character via the ITM channel 0, and
-    \li Just returns when no debugger is connected that has booked the output.
-    \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.
-
-    \param [in]     ch  Character to transmit.
-
-    \returns            Character to transmit.
- */
-__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
-{
-  if ((ITM->TCR & ITM_TCR_ITMENA_Msk)                  &&      /* ITM enabled */
-      (ITM->TER & (1UL << 0)        )                    )     /* ITM Port #0 enabled */
-  {
-    while (ITM->PORT[0].u32 == 0);
-    ITM->PORT[0].u8 = (uint8_t) ch;
-  }
-  return (ch);
-}
-
-
-/** \brief  ITM Receive Character
-
-    The function inputs a character via the external variable \ref ITM_RxBuffer.
-
-    \return             Received character.
-    \return         -1  No character pending.
- */
-__STATIC_INLINE int32_t ITM_ReceiveChar (void) {
-  int32_t ch = -1;                           /* no character available */
-
-  if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {
-    ch = ITM_RxBuffer;
-    ITM_RxBuffer = ITM_RXBUFFER_EMPTY;       /* ready for next character */
-  }
-
-  return (ch);
-}
-
-
-/** \brief  ITM Check Character
-
-    The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.
-
-    \return          0  No character available.
-    \return          1  Character available.
- */
-__STATIC_INLINE int32_t ITM_CheckChar (void) {
-
-  if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {
-    return (0);                                 /* no character available */
-  } else {
-    return (1);                                 /*    character available */
-  }
-}
-
-/*@} end of CMSIS_core_DebugFunctions */
-
-#endif /* __CORE_CM4_H_DEPENDANT */
-
-#endif /* __CMSIS_GENERIC */
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
deleted file mode 100644
index af1831ee1..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
+++ /dev/null
@@ -1,673 +0,0 @@
-/**************************************************************************//**
- * @file     core_cm4_simd.h
- * @brief    CMSIS Cortex-M4 SIMD Header File
- * @version  V3.20
- * @date     25. February 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
-   All rights reserved.
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-   - Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   - 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.
-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
-   ---------------------------------------------------------------------------*/
-
-
-#ifdef __cplusplus
- extern "C" {
-#endif
-
-#ifndef __CORE_CM4_SIMD_H
-#define __CORE_CM4_SIMD_H
-
-
-/*******************************************************************************
- *                Hardware Abstraction Layer
- ******************************************************************************/
-
-
-/* ###################  Compiler specific Intrinsics  ########################### */
-/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
-  Access to dedicated SIMD instructions
-  @{
-*/
-
-#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
-/* ARM armcc specific functions */
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-#define __SADD8                           __sadd8
-#define __QADD8                           __qadd8
-#define __SHADD8                          __shadd8
-#define __UADD8                           __uadd8
-#define __UQADD8                          __uqadd8
-#define __UHADD8                          __uhadd8
-#define __SSUB8                           __ssub8
-#define __QSUB8                           __qsub8
-#define __SHSUB8                          __shsub8
-#define __USUB8                           __usub8
-#define __UQSUB8                          __uqsub8
-#define __UHSUB8                          __uhsub8
-#define __SADD16                          __sadd16
-#define __QADD16                          __qadd16
-#define __SHADD16                         __shadd16
-#define __UADD16                          __uadd16
-#define __UQADD16                         __uqadd16
-#define __UHADD16                         __uhadd16
-#define __SSUB16                          __ssub16
-#define __QSUB16                          __qsub16
-#define __SHSUB16                         __shsub16
-#define __USUB16                          __usub16
-#define __UQSUB16                         __uqsub16
-#define __UHSUB16                         __uhsub16
-#define __SASX                            __sasx
-#define __QASX                            __qasx
-#define __SHASX                           __shasx
-#define __UASX                            __uasx
-#define __UQASX                           __uqasx
-#define __UHASX                           __uhasx
-#define __SSAX                            __ssax
-#define __QSAX                            __qsax
-#define __SHSAX                           __shsax
-#define __USAX                            __usax
-#define __UQSAX                           __uqsax
-#define __UHSAX                           __uhsax
-#define __USAD8                           __usad8
-#define __USADA8                          __usada8
-#define __SSAT16                          __ssat16
-#define __USAT16                          __usat16
-#define __UXTB16                          __uxtb16
-#define __UXTAB16                         __uxtab16
-#define __SXTB16                          __sxtb16
-#define __SXTAB16                         __sxtab16
-#define __SMUAD                           __smuad
-#define __SMUADX                          __smuadx
-#define __SMLAD                           __smlad
-#define __SMLADX                          __smladx
-#define __SMLALD                          __smlald
-#define __SMLALDX                         __smlaldx
-#define __SMUSD                           __smusd
-#define __SMUSDX                          __smusdx
-#define __SMLSD                           __smlsd
-#define __SMLSDX                          __smlsdx
-#define __SMLSLD                          __smlsld
-#define __SMLSLDX                         __smlsldx
-#define __SEL                             __sel
-#define __QADD                            __qadd
-#define __QSUB                            __qsub
-
-#define __PKHBT(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0x0000FFFFUL) |  \
-                                           ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL)  )
-
-#define __PKHTB(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0xFFFF0000UL) |  \
-                                           ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL)  )
-
-#define __SMMLA(ARG1,ARG2,ARG3)          ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \
-                                                      ((int64_t)(ARG3) << 32)      ) >> 32))
-
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-
-#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
-/* IAR iccarm specific functions */
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-#include <cmsis_iar.h>
-
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-
-#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
-/* TI CCS specific functions */
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-#include <cmsis_ccs.h>
-
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-
-#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
-/* GNU gcc specific functions */
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SASX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QASX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHASX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UASX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQASX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHASX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSAX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSAX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAD8(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3)
-{
-  uint32_t result;
-
-  __ASM volatile ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
-  return(result);
-}
-
-#define __SSAT16(ARG1,ARG2) \
-({                          \
-  uint32_t __RES, __ARG1 = (ARG1); \
-  __ASM ("ssat16 %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
-  __RES; \
- })
-
-#define __USAT16(ARG1,ARG2) \
-({                          \
-  uint32_t __RES, __ARG1 = (ARG1); \
-  __ASM ("usat16 %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
-  __RES; \
- })
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTB16(uint32_t op1)
-{
-  uint32_t result;
-
-  __ASM volatile ("uxtb16 %0, %1" : "=r" (result) : "r" (op1));
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTB16(uint32_t op1)
-{
-  uint32_t result;
-
-  __ASM volatile ("sxtb16 %0, %1" : "=r" (result) : "r" (op1));
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUAD  (uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3)
-{
-  uint32_t result;
-
-  __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3)
-{
-  uint32_t result;
-
-  __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
-  return(result);
-}
-
-#define __SMLALD(ARG1,ARG2,ARG3) \
-({ \
-  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
-  __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
-  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
- })
-
-#define __SMLALDX(ARG1,ARG2,ARG3) \
-({ \
-  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
-  __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
-  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
- })
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSD  (uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3)
-{
-  uint32_t result;
-
-  __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3)
-{
-  uint32_t result;
-
-  __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
-  return(result);
-}
-
-#define __SMLSLD(ARG1,ARG2,ARG3) \
-({ \
-  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
-  __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
-  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
- })
-
-#define __SMLSLDX(ARG1,ARG2,ARG3) \
-({ \
-  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
-  __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
-  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
- })
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SEL  (uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB(uint32_t op1, uint32_t op2)
-{
-  uint32_t result;
-
-  __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
-  return(result);
-}
-
-#define __PKHBT(ARG1,ARG2,ARG3) \
-({                          \
-  uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
-  __ASM ("pkhbt %0, %1, %2, lsl %3" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2), "I" (ARG3)  ); \
-  __RES; \
- })
-
-#define __PKHTB(ARG1,ARG2,ARG3) \
-({                          \
-  uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
-  if (ARG3 == 0) \
-    __ASM ("pkhtb %0, %1, %2" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2)  ); \
-  else \
-    __ASM ("pkhtb %0, %1, %2, asr %3" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2), "I" (ARG3)  ); \
-  __RES; \
- })
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3)
-{
- int32_t result;
-
- __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r"  (op1), "r" (op2), "r" (op3) );
- return(result);
-}
-
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-
-#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
-/* TASKING carm specific functions */
-
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-/* not yet supported */
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-#endif
-
-/*@} end of group CMSIS_SIMD_intrinsics */
-
-
-#endif /* __CORE_CM4_SIMD_H */
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/modules/mathlib/CMSIS/Include/core_cmFunc.h
deleted file mode 100644
index 139bc3c5e..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h
+++ /dev/null
@@ -1,636 +0,0 @@
-/**************************************************************************//**
- * @file     core_cmFunc.h
- * @brief    CMSIS Cortex-M Core Function Access Header File
- * @version  V3.20
- * @date     25. February 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
-   All rights reserved.
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-   - Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   - 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.
-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND CONTRIBUTORS BE
-   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-   POSSIBILITY OF SUCH DAMAGE.
-   ---------------------------------------------------------------------------*/
-
-
-#ifndef __CORE_CMFUNC_H
-#define __CORE_CMFUNC_H
-
-
-/* ###########################  Core Function Access  ########################### */
-/** \ingroup  CMSIS_Core_FunctionInterface
-    \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
-  @{
- */
-
-#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
-/* ARM armcc specific functions */
-
-#if (__ARMCC_VERSION < 400677)
-  #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
-#endif
-
-/* intrinsic void __enable_irq();     */
-/* intrinsic void __disable_irq();    */
-
-/** \brief  Get Control Register
-
-    This function returns the content of the Control Register.
-
-    \return               Control Register value
- */
-__STATIC_INLINE uint32_t __get_CONTROL(void)
-{
-  register uint32_t __regControl         __ASM("control");
-  return(__regControl);
-}
-
-
-/** \brief  Set Control Register
-
-    This function writes the given value to the Control Register.
-
-    \param [in]    control  Control Register value to set
- */
-__STATIC_INLINE void __set_CONTROL(uint32_t control)
-{
-  register uint32_t __regControl         __ASM("control");
-  __regControl = control;
-}
-
-
-/** \brief  Get IPSR Register
-
-    This function returns the content of the IPSR Register.
-
-    \return               IPSR Register value
- */
-__STATIC_INLINE uint32_t __get_IPSR(void)
-{
-  register uint32_t __regIPSR          __ASM("ipsr");
-  return(__regIPSR);
-}
-
-
-/** \brief  Get APSR Register
-
-    This function returns the content of the APSR Register.
-
-    \return               APSR Register value
- */
-__STATIC_INLINE uint32_t __get_APSR(void)
-{
-  register uint32_t __regAPSR          __ASM("apsr");
-  return(__regAPSR);
-}
-
-
-/** \brief  Get xPSR Register
-
-    This function returns the content of the xPSR Register.
-
-    \return               xPSR Register value
- */
-__STATIC_INLINE uint32_t __get_xPSR(void)
-{
-  register uint32_t __regXPSR          __ASM("xpsr");
-  return(__regXPSR);
-}
-
-
-/** \brief  Get Process Stack Pointer
-
-    This function returns the current value of the Process Stack Pointer (PSP).
-
-    \return               PSP Register value
- */
-__STATIC_INLINE uint32_t __get_PSP(void)
-{
-  register uint32_t __regProcessStackPointer  __ASM("psp");
-  return(__regProcessStackPointer);
-}
-
-
-/** \brief  Set Process Stack Pointer
-
-    This function assigns the given value to the Process Stack Pointer (PSP).
-
-    \param [in]    topOfProcStack  Process Stack Pointer value to set
- */
-__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
-{
-  register uint32_t __regProcessStackPointer  __ASM("psp");
-  __regProcessStackPointer = topOfProcStack;
-}
-
-
-/** \brief  Get Main Stack Pointer
-
-    This function returns the current value of the Main Stack Pointer (MSP).
-
-    \return               MSP Register value
- */
-__STATIC_INLINE uint32_t __get_MSP(void)
-{
-  register uint32_t __regMainStackPointer     __ASM("msp");
-  return(__regMainStackPointer);
-}
-
-
-/** \brief  Set Main Stack Pointer
-
-    This function assigns the given value to the Main Stack Pointer (MSP).
-
-    \param [in]    topOfMainStack  Main Stack Pointer value to set
- */
-__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
-{
-  register uint32_t __regMainStackPointer     __ASM("msp");
-  __regMainStackPointer = topOfMainStack;
-}
-
-
-/** \brief  Get Priority Mask
-
-    This function returns the current state of the priority mask bit from the Priority Mask Register.
-
-    \return               Priority Mask value
- */
-__STATIC_INLINE uint32_t __get_PRIMASK(void)
-{
-  register uint32_t __regPriMask         __ASM("primask");
-  return(__regPriMask);
-}
-
-
-/** \brief  Set Priority Mask
-
-    This function assigns the given value to the Priority Mask Register.
-
-    \param [in]    priMask  Priority Mask
- */
-__STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
-{
-  register uint32_t __regPriMask         __ASM("primask");
-  __regPriMask = (priMask);
-}
-
-
-#if       (__CORTEX_M >= 0x03)
-
-/** \brief  Enable FIQ
-
-    This function enables FIQ interrupts by clearing the F-bit in the CPSR.
-    Can only be executed in Privileged modes.
- */
-#define __enable_fault_irq                __enable_fiq
-
-
-/** \brief  Disable FIQ
-
-    This function disables FIQ interrupts by setting the F-bit in the CPSR.
-    Can only be executed in Privileged modes.
- */
-#define __disable_fault_irq               __disable_fiq
-
-
-/** \brief  Get Base Priority
-
-    This function returns the current value of the Base Priority register.
-
-    \return               Base Priority register value
- */
-__STATIC_INLINE uint32_t  __get_BASEPRI(void)
-{
-  register uint32_t __regBasePri         __ASM("basepri");
-  return(__regBasePri);
-}
-
-
-/** \brief  Set Base Priority
-
-    This function assigns the given value to the Base Priority register.
-
-    \param [in]    basePri  Base Priority value to set
- */
-__STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
-{
-  register uint32_t __regBasePri         __ASM("basepri");
-  __regBasePri = (basePri & 0xff);
-}
-
-
-/** \brief  Get Fault Mask
-
-    This function returns the current value of the Fault Mask register.
-
-    \return               Fault Mask register value
- */
-__STATIC_INLINE uint32_t __get_FAULTMASK(void)
-{
-  register uint32_t __regFaultMask       __ASM("faultmask");
-  return(__regFaultMask);
-}
-
-
-/** \brief  Set Fault Mask
-
-    This function assigns the given value to the Fault Mask register.
-
-    \param [in]    faultMask  Fault Mask value to set
- */
-__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
-{
-  register uint32_t __regFaultMask       __ASM("faultmask");
-  __regFaultMask = (faultMask & (uint32_t)1);
-}
-
-#endif /* (__CORTEX_M >= 0x03) */
-
-
-#if       (__CORTEX_M == 0x04)
-
-/** \brief  Get FPSCR
-
-    This function returns the current value of the Floating Point Status/Control register.
-
-    \return               Floating Point Status/Control register value
- */
-__STATIC_INLINE uint32_t __get_FPSCR(void)
-{
-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
-  register uint32_t __regfpscr         __ASM("fpscr");
-  return(__regfpscr);
-#else
-   return(0);
-#endif
-}
-
-
-/** \brief  Set FPSCR
-
-    This function assigns the given value to the Floating Point Status/Control register.
-
-    \param [in]    fpscr  Floating Point Status/Control value to set
- */
-__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
-{
-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
-  register uint32_t __regfpscr         __ASM("fpscr");
-  __regfpscr = (fpscr);
-#endif
-}
-
-#endif /* (__CORTEX_M == 0x04) */
-
-
-#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
-/* IAR iccarm specific functions */
-
-#include <cmsis_iar.h>
-
-
-#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
-/* TI CCS specific functions */
-
-#include <cmsis_ccs.h>
-
-
-#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
-/* GNU gcc specific functions */
-
-/** \brief  Enable IRQ Interrupts
-
-  This function enables IRQ interrupts by clearing the I-bit in the CPSR.
-  Can only be executed in Privileged modes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void)
-{
-  __ASM volatile ("cpsie i" : : : "memory");
-}
-
-
-/** \brief  Disable IRQ Interrupts
-
-  This function disables IRQ interrupts by setting the I-bit in the CPSR.
-  Can only be executed in Privileged modes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void)
-{
-  __ASM volatile ("cpsid i" : : : "memory");
-}
-
-
-/** \brief  Get Control Register
-
-    This function returns the content of the Control Register.
-
-    \return               Control Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void)
-{
-  uint32_t result;
-
-  __ASM volatile ("MRS %0, control" : "=r" (result) );
-  return(result);
-}
-
-
-/** \brief  Set Control Register
-
-    This function writes the given value to the Control Register.
-
-    \param [in]    control  Control Register value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control)
-{
-  __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");
-}
-
-
-/** \brief  Get IPSR Register
-
-    This function returns the content of the IPSR Register.
-
-    \return               IPSR Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void)
-{
-  uint32_t result;
-
-  __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
-  return(result);
-}
-
-
-/** \brief  Get APSR Register
-
-    This function returns the content of the APSR Register.
-
-    \return               APSR Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void)
-{
-  uint32_t result;
-
-  __ASM volatile ("MRS %0, apsr" : "=r" (result) );
-  return(result);
-}
-
-
-/** \brief  Get xPSR Register
-
-    This function returns the content of the xPSR Register.
-
-    \return               xPSR Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void)
-{
-  uint32_t result;
-
-  __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
-  return(result);
-}
-
-
-/** \brief  Get Process Stack Pointer
-
-    This function returns the current value of the Process Stack Pointer (PSP).
-
-    \return               PSP Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)
-{
-  register uint32_t result;
-
-  __ASM volatile ("MRS %0, psp\n"  : "=r" (result) );
-  return(result);
-}
-
-
-/** \brief  Set Process Stack Pointer
-
-    This function assigns the given value to the Process Stack Pointer (PSP).
-
-    \param [in]    topOfProcStack  Process Stack Pointer value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
-{
-  __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp");
-}
-
-
-/** \brief  Get Main Stack Pointer
-
-    This function returns the current value of the Main Stack Pointer (MSP).
-
-    \return               MSP Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)
-{
-  register uint32_t result;
-
-  __ASM volatile ("MRS %0, msp\n" : "=r" (result) );
-  return(result);
-}
-
-
-/** \brief  Set Main Stack Pointer
-
-    This function assigns the given value to the Main Stack Pointer (MSP).
-
-    \param [in]    topOfMainStack  Main Stack Pointer value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
-{
-  __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp");
-}
-
-
-/** \brief  Get Priority Mask
-
-    This function returns the current state of the priority mask bit from the Priority Mask Register.
-
-    \return               Priority Mask value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void)
-{
-  uint32_t result;
-
-  __ASM volatile ("MRS %0, primask" : "=r" (result) );
-  return(result);
-}
-
-
-/** \brief  Set Priority Mask
-
-    This function assigns the given value to the Priority Mask Register.
-
-    \param [in]    priMask  Priority Mask
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
-{
-  __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");
-}
-
-
-#if       (__CORTEX_M >= 0x03)
-
-/** \brief  Enable FIQ
-
-    This function enables FIQ interrupts by clearing the F-bit in the CPSR.
-    Can only be executed in Privileged modes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void)
-{
-  __ASM volatile ("cpsie f" : : : "memory");
-}
-
-
-/** \brief  Disable FIQ
-
-    This function disables FIQ interrupts by setting the F-bit in the CPSR.
-    Can only be executed in Privileged modes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void)
-{
-  __ASM volatile ("cpsid f" : : : "memory");
-}
-
-
-/** \brief  Get Base Priority
-
-    This function returns the current value of the Base Priority register.
-
-    \return               Base Priority register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void)
-{
-  uint32_t result;
-
-  __ASM volatile ("MRS %0, basepri_max" : "=r" (result) );
-  return(result);
-}
-
-
-/** \brief  Set Base Priority
-
-    This function assigns the given value to the Base Priority register.
-
-    \param [in]    basePri  Base Priority value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value)
-{
-  __ASM volatile ("MSR basepri, %0" : : "r" (value) : "memory");
-}
-
-
-/** \brief  Get Fault Mask
-
-    This function returns the current value of the Fault Mask register.
-
-    \return               Fault Mask register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void)
-{
-  uint32_t result;
-
-  __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
-  return(result);
-}
-
-
-/** \brief  Set Fault Mask
-
-    This function assigns the given value to the Fault Mask register.
-
-    \param [in]    faultMask  Fault Mask value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
-{
-  __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");
-}
-
-#endif /* (__CORTEX_M >= 0x03) */
-
-
-#if       (__CORTEX_M == 0x04)
-
-/** \brief  Get FPSCR
-
-    This function returns the current value of the Floating Point Status/Control register.
-
-    \return               Floating Point Status/Control register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void)
-{
-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
-  uint32_t result;
-
-  /* Empty asm statement works as a scheduling barrier */
-  __ASM volatile ("");
-  __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
-  __ASM volatile ("");
-  return(result);
-#else
-   return(0);
-#endif
-}
-
-
-/** \brief  Set FPSCR
-
-    This function assigns the given value to the Floating Point Status/Control register.
-
-    \param [in]    fpscr  Floating Point Status/Control value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
-{
-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
-  /* Empty asm statement works as a scheduling barrier */
-  __ASM volatile ("");
-  __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc");
-  __ASM volatile ("");
-#endif
-}
-
-#endif /* (__CORTEX_M == 0x04) */
-
-
-#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
-/* TASKING carm specific functions */
-
-/*
- * The CMSIS functions have been implemented as intrinsics in the compiler.
- * Please use "carm -?i" to get an up to date list of all instrinsics,
- * Including the CMSIS ones.
- */
-
-#endif
-
-/*@} end of CMSIS_Core_RegAccFunctions */
-
-
-#endif /* __CORE_CMFUNC_H */
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/modules/mathlib/CMSIS/Include/core_cmInstr.h
deleted file mode 100644
index 8946c2c49..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h
+++ /dev/null
@@ -1,688 +0,0 @@
-/**************************************************************************//**
- * @file     core_cmInstr.h
- * @brief    CMSIS Cortex-M Core Instruction Access Header File
- * @version  V3.20
- * @date     05. March 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
-   All rights reserved.
-   Redistribution and use in source and binary forms, with or without
-   modification, are permitted provided that the following conditions are met:
-   - Redistributions of source code must retain the above copyright
-     notice, this list of conditions and the following disclaimer.
-   - 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.
-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND CONTRIBUTORS BE
-   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-   POSSIBILITY OF SUCH DAMAGE.
-   ---------------------------------------------------------------------------*/
-
-
-#ifndef __CORE_CMINSTR_H
-#define __CORE_CMINSTR_H
-
-
-/* ##########################  Core Instruction Access  ######################### */
-/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
-  Access to dedicated instructions
-  @{
-*/
-
-#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
-/* ARM armcc specific functions */
-
-#if (__ARMCC_VERSION < 400677)
-  #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
-#endif
-
-
-/** \brief  No Operation
-
-    No Operation does nothing. This instruction can be used for code alignment purposes.
- */
-#define __NOP                             __nop
-
-
-/** \brief  Wait For Interrupt
-
-    Wait For Interrupt is a hint instruction that suspends execution
-    until one of a number of events occurs.
- */
-#define __WFI                             __wfi
-
-
-/** \brief  Wait For Event
-
-    Wait For Event is a hint instruction that permits the processor to enter
-    a low-power state until one of a number of events occurs.
- */
-#define __WFE                             __wfe
-
-
-/** \brief  Send Event
-
-    Send Event is a hint instruction. It causes an event to be signaled to the CPU.
- */
-#define __SEV                             __sev
-
-
-/** \brief  Instruction Synchronization Barrier
-
-    Instruction Synchronization Barrier flushes the pipeline in the processor,
-    so that all instructions following the ISB are fetched from cache or
-    memory, after the instruction has been completed.
- */
-#define __ISB()                           __isb(0xF)
-
-
-/** \brief  Data Synchronization Barrier
-
-    This function acts as a special kind of Data Memory Barrier.
-    It completes when all explicit memory accesses before this instruction complete.
- */
-#define __DSB()                           __dsb(0xF)
-
-
-/** \brief  Data Memory Barrier
-
-    This function ensures the apparent order of the explicit memory operations before
-    and after the instruction, without ensuring their completion.
- */
-#define __DMB()                           __dmb(0xF)
-
-
-/** \brief  Reverse byte order (32 bit)
-
-    This function reverses the byte order in integer value.
-
-    \param [in]    value  Value to reverse
-    \return               Reversed value
- */
-#define __REV                             __rev
-
-
-/** \brief  Reverse byte order (16 bit)
-
-    This function reverses the byte order in two unsigned short values.
-
-    \param [in]    value  Value to reverse
-    \return               Reversed value
- */
-#ifndef __NO_EMBEDDED_ASM
-__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)
-{
-  rev16 r0, r0
-  bx lr
-}
-#endif
-
-/** \brief  Reverse byte order in signed short value
-
-    This function reverses the byte order in a signed short value with sign extension to integer.
-
-    \param [in]    value  Value to reverse
-    \return               Reversed value
- */
-#ifndef __NO_EMBEDDED_ASM
-__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int32_t __REVSH(int32_t value)
-{
-  revsh r0, r0
-  bx lr
-}
-#endif
-
-
-/** \brief  Rotate Right in unsigned value (32 bit)
-
-    This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
-
-    \param [in]    value  Value to rotate
-    \param [in]    value  Number of Bits to rotate
-    \return               Rotated value
- */
-#define __ROR                             __ror
-
-
-/** \brief  Breakpoint
-
-    This function causes the processor to enter Debug state.
-    Debug tools can use this to investigate system state when the instruction at a particular address is reached.
-
-    \param [in]    value  is ignored by the processor.
-                   If required, a debugger can use it to store additional information about the breakpoint.
- */
-#define __BKPT(value)                       __breakpoint(value)
-
-
-#if       (__CORTEX_M >= 0x03)
-
-/** \brief  Reverse bit order of value
-
-    This function reverses the bit order of the given value.
-
-    \param [in]    value  Value to reverse
-    \return               Reversed value
- */
-#define __RBIT                            __rbit
-
-
-/** \brief  LDR Exclusive (8 bit)
-
-    This function performs a exclusive LDR command for 8 bit value.
-
-    \param [in]    ptr  Pointer to data
-    \return             value of type uint8_t at (*ptr)
- */
-#define __LDREXB(ptr)                     ((uint8_t ) __ldrex(ptr))
-
-
-/** \brief  LDR Exclusive (16 bit)
-
-    This function performs a exclusive LDR command for 16 bit values.
-
-    \param [in]    ptr  Pointer to data
-    \return        value of type uint16_t at (*ptr)
- */
-#define __LDREXH(ptr)                     ((uint16_t) __ldrex(ptr))
-
-
-/** \brief  LDR Exclusive (32 bit)
-
-    This function performs a exclusive LDR command for 32 bit values.
-
-    \param [in]    ptr  Pointer to data
-    \return        value of type uint32_t at (*ptr)
- */
-#define __LDREXW(ptr)                     ((uint32_t ) __ldrex(ptr))
-
-
-/** \brief  STR Exclusive (8 bit)
-
-    This function performs a exclusive STR command for 8 bit values.
-
-    \param [in]  value  Value to store
-    \param [in]    ptr  Pointer to location
-    \return          0  Function succeeded
-    \return          1  Function failed
- */
-#define __STREXB(value, ptr)              __strex(value, ptr)
-
-
-/** \brief  STR Exclusive (16 bit)
-
-    This function performs a exclusive STR command for 16 bit values.
-
-    \param [in]  value  Value to store
-    \param [in]    ptr  Pointer to location
-    \return          0  Function succeeded
-    \return          1  Function failed
- */
-#define __STREXH(value, ptr)              __strex(value, ptr)
-
-
-/** \brief  STR Exclusive (32 bit)
-
-    This function performs a exclusive STR command for 32 bit values.
-
-    \param [in]  value  Value to store
-    \param [in]    ptr  Pointer to location
-    \return          0  Function succeeded
-    \return          1  Function failed
- */
-#define __STREXW(value, ptr)              __strex(value, ptr)
-
-
-/** \brief  Remove the exclusive lock
-
-    This function removes the exclusive lock which is created by LDREX.
-
- */
-#define __CLREX                           __clrex
-
-
-/** \brief  Signed Saturate
-
-    This function saturates a signed value.
-
-    \param [in]  value  Value to be saturated
-    \param [in]    sat  Bit position to saturate to (1..32)
-    \return             Saturated value
- */
-#define __SSAT                            __ssat
-
-
-/** \brief  Unsigned Saturate
-
-    This function saturates an unsigned value.
-
-    \param [in]  value  Value to be saturated
-    \param [in]    sat  Bit position to saturate to (0..31)
-    \return             Saturated value
- */
-#define __USAT                            __usat
-
-
-/** \brief  Count leading zeros
-
-    This function counts the number of leading zeros of a data value.
-
-    \param [in]  value  Value to count the leading zeros
-    \return             number of leading zeros in value
- */
-#define __CLZ                             __clz
-
-#endif /* (__CORTEX_M >= 0x03) */
-
-
-
-#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
-/* IAR iccarm specific functions */
-
-#include <cmsis_iar.h>
-
-
-#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
-/* TI CCS specific functions */
-
-#include <cmsis_ccs.h>
-
-
-#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
-/* GNU gcc specific functions */
-
-/* Define macros for porting to both thumb1 and thumb2.
- * For thumb1, use low register (r0-r7), specified by constrant "l"
- * Otherwise, use general registers, specified by constrant "r" */
-#if defined (__thumb__) && !defined (__thumb2__)
-#define __CMSIS_GCC_OUT_REG(r) "=l" (r)
-#define __CMSIS_GCC_USE_REG(r) "l" (r)
-#else
-#define __CMSIS_GCC_OUT_REG(r) "=r" (r)
-#define __CMSIS_GCC_USE_REG(r) "r" (r)
-#endif
-
-/** \brief  No Operation
-
-    No Operation does nothing. This instruction can be used for code alignment purposes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void)
-{
-  __ASM volatile ("nop");
-}
-
-
-/** \brief  Wait For Interrupt
-
-    Wait For Interrupt is a hint instruction that suspends execution
-    until one of a number of events occurs.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void)
-{
-  __ASM volatile ("wfi");
-}
-
-
-/** \brief  Wait For Event
-
-    Wait For Event is a hint instruction that permits the processor to enter
-    a low-power state until one of a number of events occurs.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void)
-{
-  __ASM volatile ("wfe");
-}
-
-
-/** \brief  Send Event
-
-    Send Event is a hint instruction. It causes an event to be signaled to the CPU.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void)
-{
-  __ASM volatile ("sev");
-}
-
-
-/** \brief  Instruction Synchronization Barrier
-
-    Instruction Synchronization Barrier flushes the pipeline in the processor,
-    so that all instructions following the ISB are fetched from cache or
-    memory, after the instruction has been completed.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void)
-{
-  __ASM volatile ("isb");
-}
-
-
-/** \brief  Data Synchronization Barrier
-
-    This function acts as a special kind of Data Memory Barrier.
-    It completes when all explicit memory accesses before this instruction complete.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void)
-{
-  __ASM volatile ("dsb");
-}
-
-
-/** \brief  Data Memory Barrier
-
-    This function ensures the apparent order of the explicit memory operations before
-    and after the instruction, without ensuring their completion.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void)
-{
-  __ASM volatile ("dmb");
-}
-
-
-/** \brief  Reverse byte order (32 bit)
-
-    This function reverses the byte order in integer value.
-
-    \param [in]    value  Value to reverse
-    \return               Reversed value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32_t value)
-{
-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)
-  return __builtin_bswap32(value);
-#else
-  uint32_t result;
-
-  __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
-  return(result);
-#endif
-}
-
-
-/** \brief  Reverse byte order (16 bit)
-
-    This function reverses the byte order in two unsigned short values.
-
-    \param [in]    value  Value to reverse
-    \return               Reversed value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t value)
-{
-  uint32_t result;
-
-  __ASM volatile ("rev16 %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
-  return(result);
-}
-
-
-/** \brief  Reverse byte order in signed short value
-
-    This function reverses the byte order in a signed short value with sign extension to integer.
-
-    \param [in]    value  Value to reverse
-    \return               Reversed value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value)
-{
-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
-  return (short)__builtin_bswap16(value);
-#else
-  uint32_t result;
-
-  __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
-  return(result);
-#endif
-}
-
-
-/** \brief  Rotate Right in unsigned value (32 bit)
-
-    This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
-
-    \param [in]    value  Value to rotate
-    \param [in]    value  Number of Bits to rotate
-    \return               Rotated value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
-{
-  return (op1 >> op2) | (op1 << (32 - op2)); 
-}
-
-
-/** \brief  Breakpoint
-
-    This function causes the processor to enter Debug state.
-    Debug tools can use this to investigate system state when the instruction at a particular address is reached.
-
-    \param [in]    value  is ignored by the processor.
-                   If required, a debugger can use it to store additional information about the breakpoint.
- */
-#define __BKPT(value)                       __ASM volatile ("bkpt "#value)
-
-
-#if       (__CORTEX_M >= 0x03)
-
-/** \brief  Reverse bit order of value
-
-    This function reverses the bit order of the given value.
-
-    \param [in]    value  Value to reverse
-    \return               Reversed value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint32_t value)
-{
-  uint32_t result;
-
-   __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) );
-   return(result);
-}
-
-
-/** \brief  LDR Exclusive (8 bit)
-
-    This function performs a exclusive LDR command for 8 bit value.
-
-    \param [in]    ptr  Pointer to data
-    \return             value of type uint8_t at (*ptr)
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uint8_t *addr)
-{
-    uint32_t result;
-
-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
-   __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) );
-#else
-    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
-       accepted by assembler. So has to use following less efficient pattern.
-    */
-   __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
-#endif
-   return(result);
-}
-
-
-/** \brief  LDR Exclusive (16 bit)
-
-    This function performs a exclusive LDR command for 16 bit values.
-
-    \param [in]    ptr  Pointer to data
-    \return        value of type uint16_t at (*ptr)
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile uint16_t *addr)
-{
-    uint32_t result;
-
-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
-   __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) );
-#else
-    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
-       accepted by assembler. So has to use following less efficient pattern.
-    */
-   __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
-#endif
-   return(result);
-}
-
-
-/** \brief  LDR Exclusive (32 bit)
-
-    This function performs a exclusive LDR command for 32 bit values.
-
-    \param [in]    ptr  Pointer to data
-    \return        value of type uint32_t at (*ptr)
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(volatile uint32_t *addr)
-{
-    uint32_t result;
-
-   __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) );
-   return(result);
-}
-
-
-/** \brief  STR Exclusive (8 bit)
-
-    This function performs a exclusive STR command for 8 bit values.
-
-    \param [in]  value  Value to store
-    \param [in]    ptr  Pointer to location
-    \return          0  Function succeeded
-    \return          1  Function failed
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)
-{
-   uint32_t result;
-
-   __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
-   return(result);
-}
-
-
-/** \brief  STR Exclusive (16 bit)
-
-    This function performs a exclusive STR command for 16 bit values.
-
-    \param [in]  value  Value to store
-    \param [in]    ptr  Pointer to location
-    \return          0  Function succeeded
-    \return          1  Function failed
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)
-{
-   uint32_t result;
-
-   __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
-   return(result);
-}
-
-
-/** \brief  STR Exclusive (32 bit)
-
-    This function performs a exclusive STR command for 32 bit values.
-
-    \param [in]  value  Value to store
-    \param [in]    ptr  Pointer to location
-    \return          0  Function succeeded
-    \return          1  Function failed
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)
-{
-   uint32_t result;
-
-   __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
-   return(result);
-}
-
-
-/** \brief  Remove the exclusive lock
-
-    This function removes the exclusive lock which is created by LDREX.
-
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void)
-{
-  __ASM volatile ("clrex" ::: "memory");
-}
-
-
-/** \brief  Signed Saturate
-
-    This function saturates a signed value.
-
-    \param [in]  value  Value to be saturated
-    \param [in]    sat  Bit position to saturate to (1..32)
-    \return             Saturated value
- */
-#define __SSAT(ARG1,ARG2) \
-({                          \
-  uint32_t __RES, __ARG1 = (ARG1); \
-  __ASM ("ssat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
-  __RES; \
- })
-
-
-/** \brief  Unsigned Saturate
-
-    This function saturates an unsigned value.
-
-    \param [in]  value  Value to be saturated
-    \param [in]    sat  Bit position to saturate to (0..31)
-    \return             Saturated value
- */
-#define __USAT(ARG1,ARG2) \
-({                          \
-  uint32_t __RES, __ARG1 = (ARG1); \
-  __ASM ("usat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
-  __RES; \
- })
-
-
-/** \brief  Count leading zeros
-
-    This function counts the number of leading zeros of a data value.
-
-    \param [in]  value  Value to count the leading zeros
-    \return             number of leading zeros in value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value)
-{
-   uint32_t result;
-
-  __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
-  return(result);
-}
-
-#endif /* (__CORTEX_M >= 0x03) */
-
-
-
-
-#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
-/* TASKING carm specific functions */
-
-/*
- * The CMSIS functions have been implemented as intrinsics in the compiler.
- * Please use "carm -?i" to get an up to date list of all intrinsics,
- * Including the CMSIS ones.
- */
-
-#endif
-
-/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
-
-#endif /* __CORE_CMINSTR_H */
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a
deleted file mode 100644
index 6898bc27d..000000000
Binary files a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a and /dev/null differ
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a
deleted file mode 100755
index a0185eaa9..000000000
Binary files a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a and /dev/null differ
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a
deleted file mode 100755
index 94525528e..000000000
Binary files a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a and /dev/null differ
diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/modules/mathlib/CMSIS/library.mk
deleted file mode 100644
index 0cc1b559d..000000000
--- a/src/modules/mathlib/CMSIS/library.mk
+++ /dev/null
@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# ARM CMSIS DSP library
-#
-
-ifeq ($(CONFIG_ARCH),CORTEXM4F)
-PREBUILT_LIB		:= libarm_cortexM4lf_math.a
-else ifeq ($(CONFIG_ARCH),CORTEXM4)
-PREBUILT_LIB		:= libarm_cortexM4l_math.a
-else ifeq ($(CONFIG_ARCH),CORTEXM3)
-PREBUILT_LIB		:= libarm_cortexM3l_math.a
-else
-$(error CONFIG_ARCH value '$(CONFIG_ARCH)' not supported by the DSP library)
-endif
diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/modules/mathlib/CMSIS/license.txt
deleted file mode 100644
index 31afac1ec..000000000
--- a/src/modules/mathlib/CMSIS/license.txt
+++ /dev/null
@@ -1,27 +0,0 @@
-All pre-built libraries are guided by the following license:
-
-Copyright (C) 2009-2012 ARM Limited. 
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
- - Redistributions of source code must retain the above copyright
-   notice, this list of conditions and the following disclaimer.
- - 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.
- - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp
deleted file mode 100644
index f509f7081..000000000
--- a/src/modules/mathlib/math/Dcm.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-/****************************************************************************
- *
- *   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 Dcm.cpp
- *
- * math direction cosine matrix
- */
-
-#include <mathlib/math/test/test.hpp>
-
-#include "Dcm.hpp"
-#include "Quaternion.hpp"
-#include "EulerAngles.hpp"
-#include "Vector3.hpp"
-
-namespace math
-{
-
-Dcm::Dcm() :
-	Matrix(Matrix::identity(3))
-{
-}
-
-Dcm::Dcm(float c00, float c01, float c02,
-	 float c10, float c11, float c12,
-	 float c20, float c21, float c22) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	dcm(0, 0) = c00;
-	dcm(0, 1) = c01;
-	dcm(0, 2) = c02;
-	dcm(1, 0) = c10;
-	dcm(1, 1) = c11;
-	dcm(1, 2) = c12;
-	dcm(2, 0) = c20;
-	dcm(2, 1) = c21;
-	dcm(2, 2) = c22;
-}
-
-Dcm::Dcm(const float data[3][3]) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	/* set rotation matrix */
-	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-		dcm(i, j) = data[i][j];
-}
-
-Dcm::Dcm(const float *data) :
-	Matrix(3, 3, data)
-{
-}
-
-Dcm::Dcm(const Quaternion &q) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	double a = q.getA();
-	double b = q.getB();
-	double c = q.getC();
-	double d = q.getD();
-	double aSq = a * a;
-	double bSq = b * b;
-	double cSq = c * c;
-	double dSq = d * d;
-	dcm(0, 0) = aSq + bSq - cSq - dSq;
-	dcm(0, 1) = 2.0 * (b * c - a * d);
-	dcm(0, 2) = 2.0 * (a * c + b * d);
-	dcm(1, 0) = 2.0 * (b * c + a * d);
-	dcm(1, 1) = aSq - bSq + cSq - dSq;
-	dcm(1, 2) = 2.0 * (c * d - a * b);
-	dcm(2, 0) = 2.0 * (b * d - a * c);
-	dcm(2, 1) = 2.0 * (a * b + c * d);
-	dcm(2, 2) = aSq - bSq - cSq + dSq;
-}
-
-Dcm::Dcm(const EulerAngles &euler) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	double cosPhi = cos(euler.getPhi());
-	double sinPhi = sin(euler.getPhi());
-	double cosThe = cos(euler.getTheta());
-	double sinThe = sin(euler.getTheta());
-	double cosPsi = cos(euler.getPsi());
-	double sinPsi = sin(euler.getPsi());
-
-	dcm(0, 0) = cosThe * cosPsi;
-	dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
-	dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
-
-	dcm(1, 0) = cosThe * sinPsi;
-	dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
-	dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
-
-	dcm(2, 0) = -sinThe;
-	dcm(2, 1) = sinPhi * cosThe;
-	dcm(2, 2) = cosPhi * cosThe;
-}
-
-Dcm::Dcm(const Dcm &right) :
-	Matrix(right)
-{
-}
-
-Dcm::~Dcm()
-{
-}
-
-int __EXPORT dcmTest()
-{
-	printf("Test DCM\t\t: ");
-	// default ctor
-	ASSERT(matrixEqual(Dcm(),
-			   Matrix::identity(3)));
-	// quaternion ctor
-	ASSERT(matrixEqual(
-		       Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
-		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
-			   0.2896295f,  0.9564251f, -0.0369570f,
-			   -0.1986693f,  0.0978434f,  0.9751703f)));
-	// euler angle ctor
-	ASSERT(matrixEqual(
-		       Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
-		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
-			   0.2896295f,  0.9564251f, -0.0369570f,
-			   -0.1986693f,  0.0978434f,  0.9751703f)));
-	// rotations
-	Vector3 vB(1, 2, 3);
-	ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
-			   Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
-	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
-			   Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
-	ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
-			   Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
-	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
-			   Dcm(EulerAngles(
-				       M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
-	printf("PASS\n");
-	return 0;
-}
-} // namespace math
diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/mathlib/math/Dcm.hpp
deleted file mode 100644
index df8970d3a..000000000
--- a/src/modules/mathlib/math/Dcm.hpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- *   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 Dcm.hpp
- *
- * math direction cosine matrix
- */
-
-//#pragma once
-
-#include "Vector.hpp"
-#include "Matrix.hpp"
-
-namespace math
-{
-
-class Quaternion;
-class EulerAngles;
-
-/**
- * This is a Tait Bryan, Body 3-2-1 sequence.
- * (yaw)-(pitch)-(roll)
- * The Dcm transforms a vector in the body frame
- * to the navigation frame, typically represented
- * as C_nb. C_bn can be obtained through use
- * of the transpose() method.
- */
-class __EXPORT Dcm : public Matrix
-{
-public:
-	/**
-	 * default ctor
-	 */
-	Dcm();
-
-	/**
-	 * scalar ctor
-	 */
-	Dcm(float c00, float c01, float c02,
-	    float c10, float c11, float c12,
-	    float c20, float c21, float c22);
-
-	/**
-	 * data ctor
-	 */
-	Dcm(const float *data);
-
-	/**
-	 * array ctor
-	 */
-	Dcm(const float data[3][3]);
-
-	/**
-	 * quaternion ctor
-	 */
-	Dcm(const Quaternion &q);
-
-	/**
-	 * euler angles ctor
-	 */
-	Dcm(const EulerAngles &euler);
-
-	/**
-	 * copy ctor (deep)
-	 */
-	Dcm(const Dcm &right);
-
-	/**
-	 * dtor
-	 */
-	virtual ~Dcm();
-};
-
-int __EXPORT dcmTest();
-
-} // math
-
diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/modules/mathlib/math/EulerAngles.cpp
deleted file mode 100644
index e733d23bb..000000000
--- a/src/modules/mathlib/math/EulerAngles.cpp
+++ /dev/null
@@ -1,126 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-#include "EulerAngles.hpp"
-#include "Quaternion.hpp"
-#include "Dcm.hpp"
-#include "Vector3.hpp"
-
-namespace math
-{
-
-EulerAngles::EulerAngles() :
-	Vector(3)
-{
-	setPhi(0.0f);
-	setTheta(0.0f);
-	setPsi(0.0f);
-}
-
-EulerAngles::EulerAngles(float phi, float theta, float psi) :
-	Vector(3)
-{
-	setPhi(phi);
-	setTheta(theta);
-	setPsi(psi);
-}
-
-EulerAngles::EulerAngles(const Quaternion &q) :
-	Vector(3)
-{
-	(*this) = EulerAngles(Dcm(q));
-}
-
-EulerAngles::EulerAngles(const Dcm &dcm) :
-	Vector(3)
-{
-	setTheta(asinf(-dcm(2, 0)));
-
-	if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
-		setPhi(0.0f);
-		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
-			      dcm(0, 2) + dcm(1, 1)) + getPhi());
-
-	} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
-		setPhi(0.0f);
-		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
-			      dcm(0, 2) + dcm(1, 1)) - getPhi());
-
-	} else {
-		setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
-		setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
-	}
-}
-
-EulerAngles::~EulerAngles()
-{
-}
-
-int __EXPORT eulerAnglesTest()
-{
-	printf("Test EulerAngles\t: ");
-	EulerAngles euler(0.1f, 0.2f, 0.3f);
-
-	// test ctor
-	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-	ASSERT(equal(euler.getPhi(), 0.1f));
-	ASSERT(equal(euler.getTheta(), 0.2f));
-	ASSERT(equal(euler.getPsi(), 0.3f));
-
-	// test dcm ctor
-	euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-
-	// test quat ctor
-	euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-
-	// test assignment
-	euler.setPhi(0.4f);
-	euler.setTheta(0.5f);
-	euler.setPsi(0.6f);
-	ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
-
-	printf("PASS\n");
-	return 0;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/modules/mathlib/math/EulerAngles.hpp
deleted file mode 100644
index 399eecfa7..000000000
--- a/src/modules/mathlib/math/EulerAngles.hpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector.h
- *
- * math vector
- */
-
-#pragma once
-
-#include "Vector.hpp"
-
-namespace math
-{
-
-class Quaternion;
-class Dcm;
-
-class __EXPORT EulerAngles : public Vector
-{
-public:
-	EulerAngles();
-	EulerAngles(float phi, float theta, float psi);
-	EulerAngles(const Quaternion &q);
-	EulerAngles(const Dcm &dcm);
-	virtual ~EulerAngles();
-
-	// alias
-	void setPhi(float phi) { (*this)(0) = phi; }
-	void setTheta(float theta) { (*this)(1) = theta; }
-	void setPsi(float psi) { (*this)(2) = psi; }
-
-	// const accessors
-	const float &getPhi() const { return (*this)(0); }
-	const float &getTheta() const { return (*this)(1); }
-	const float &getPsi() const { return (*this)(2); }
-
-};
-
-int __EXPORT eulerAnglesTest();
-
-} // math
-
diff --git a/src/modules/mathlib/math/Limits.cpp b/src/modules/mathlib/math/Limits.cpp
deleted file mode 100644
index d4c892d8a..000000000
--- a/src/modules/mathlib/math/Limits.cpp
+++ /dev/null
@@ -1,146 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Limits.cpp
- *
- * Limiting / constrain helper functions
- */
-
-
-#include <math.h>
-#include <stdint.h>
-
-#include "Limits.hpp"
-
-
-namespace math {
-
-
-float __EXPORT min(float val1, float val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-int __EXPORT min(int val1, int val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-unsigned __EXPORT min(unsigned val1, unsigned val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-uint64_t __EXPORT min(uint64_t val1, uint64_t val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-double __EXPORT min(double val1, double val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-float __EXPORT max(float val1, float val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-int __EXPORT max(int val1, int val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-unsigned __EXPORT max(unsigned val1, unsigned val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-uint64_t __EXPORT max(uint64_t val1, uint64_t val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-double __EXPORT max(double val1, double val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-
-float __EXPORT constrain(float val, float min, float max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-int __EXPORT constrain(int val, int min, int max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-double __EXPORT constrain(double val, double min, double max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-float __EXPORT radians(float degrees)
-{
-	return (degrees / 180.0f) * M_PI_F;
-}
-
-double __EXPORT radians(double degrees)
-{
-	return (degrees / 180.0) * M_PI;
-}
-
-float __EXPORT degrees(float radians)
-{
-	return (radians / M_PI_F) * 180.0f;
-}
-
-double __EXPORT degrees(double radians)
-{
-	return (radians / M_PI) * 180.0;
-}
-
-}
\ No newline at end of file
diff --git a/src/modules/mathlib/math/Limits.hpp b/src/modules/mathlib/math/Limits.hpp
deleted file mode 100644
index fb778dd66..000000000
--- a/src/modules/mathlib/math/Limits.hpp
+++ /dev/null
@@ -1,87 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Limits.hpp
- *
- * Limiting / constrain helper functions
- */
-
-#pragma once
-
-#include <nuttx/config.h>
-#include <stdint.h>
-
-namespace math {
-
-
-float __EXPORT min(float val1, float val2);
-
-int __EXPORT min(int val1, int val2);
-
-unsigned __EXPORT min(unsigned val1, unsigned val2);
-
-uint64_t __EXPORT min(uint64_t val1, uint64_t val2);
-
-double __EXPORT min(double val1, double val2);
-
-float __EXPORT max(float val1, float val2);
-
-int __EXPORT max(int val1, int val2);
-
-unsigned __EXPORT max(unsigned val1, unsigned val2);
-
-uint64_t __EXPORT max(uint64_t val1, uint64_t val2);
-
-double __EXPORT max(double val1, double val2);
-
-
-float __EXPORT constrain(float val, float min, float max);
-
-int __EXPORT constrain(int val, int min, int max);
-
-unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max);
-
-uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max);
-
-double __EXPORT constrain(double val, double min, double max);
-
-float __EXPORT radians(float degrees);
-
-double __EXPORT radians(double degrees);
-
-float __EXPORT degrees(float radians);
-
-double __EXPORT degrees(double radians);
-
-}
\ No newline at end of file
diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/modules/mathlib/math/Matrix.cpp
deleted file mode 100644
index ebd1aeda3..000000000
--- a/src/modules/mathlib/math/Matrix.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-/****************************************************************************
- *
- *   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 Matrix.cpp
- *
- * matrix code
- */
-
-#include "test/test.hpp"
-#include <math.h>
-
-#include "Matrix.hpp"
-
-namespace math
-{
-
-static const float data_testA[] = {
-	1, 2, 3,
-	4, 5, 6
-};
-static Matrix testA(2, 3, data_testA);
-
-static const float data_testB[] = {
-	0, 1, 3,
-	7, -1, 2
-};
-static Matrix testB(2, 3, data_testB);
-
-static const float data_testC[] = {
-	0, 1,
-	2, 1,
-	3, 2
-};
-static Matrix testC(3, 2, data_testC);
-
-static const float data_testD[] = {
-	0, 1, 2,
-	2, 1, 4,
-	5, 2, 0
-};
-static Matrix testD(3, 3, data_testD);
-
-static const float data_testE[] = {
-	1, -1, 2,
-	0, 2, 3,
-	2, -1, 1
-};
-static Matrix testE(3, 3, data_testE);
-
-static const float data_testF[] = {
-	3.777e006f, 2.915e007f, 0.000e000f,
-	2.938e007f, 2.267e008f, 0.000e000f,
-	0.000e000f, 0.000e000f, 6.033e008f
-};
-static Matrix testF(3, 3, data_testF);
-
-int __EXPORT matrixTest()
-{
-	matrixAddTest();
-	matrixSubTest();
-	matrixMultTest();
-	matrixInvTest();
-	matrixDivTest();
-	return 0;
-}
-
-int matrixAddTest()
-{
-	printf("Test Matrix Add\t\t: ");
-	Matrix r = testA + testB;
-	float data_test[] = {
-		1.0f, 3.0f, 6.0f,
-		11.0f, 4.0f, 8.0f
-	};
-	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixSubTest()
-{
-	printf("Test Matrix Sub\t\t: ");
-	Matrix r = testA - testB;
-	float data_test[] = {
-		1.0f, 1.0f, 0.0f,
-		-3.0f, 6.0f, 4.0f
-	};
-	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixMultTest()
-{
-	printf("Test Matrix Mult\t: ");
-	Matrix r = testC * testB;
-	float data_test[] = {
-		7.0f, -1.0f,  2.0f,
-		7.0f,  1.0f,  8.0f,
-		14.0f,  1.0f, 13.0f
-	};
-	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixInvTest()
-{
-	printf("Test Matrix Inv\t\t: ");
-	Matrix origF = testF;
-	Matrix r = testF.inverse();
-	float data_test[] = {
-		-0.0012518f,  0.0001610f, 0.0000000f,
-		0.0001622f, -0.0000209f, 0.0000000f,
-		0.0000000f,  0.0000000f, 1.6580e-9f
-	};
-	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
-	// make sure F in unchanged
-	ASSERT(matrixEqual(origF, testF));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixDivTest()
-{
-	printf("Test Matrix Div\t\t: ");
-	Matrix r = testD / testE;
-	float data_test[] = {
-		0.2222222f, 0.5555556f, -0.1111111f,
-		0.0f,       1.0f,         1.0,
-		-4.1111111f, 1.2222222f,  4.5555556f
-	};
-	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-bool matrixEqual(const Matrix &a, const Matrix &b, float eps)
-{
-	if (a.getRows() != b.getRows()) {
-		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
-		return false;
-
-	} else if (a.getCols() != b.getCols()) {
-		printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
-		return false;
-	}
-
-	bool ret = true;
-
-	for (size_t i = 0; i < a.getRows(); i++)
-		for (size_t j = 0; j < a.getCols(); j++) {
-			if (!equal(a(i, j), b(i, j), eps)) {
-				printf("element mismatch (%d, %d)\n", i, j);
-				ret = false;
-			}
-		}
-
-	return ret;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/modules/mathlib/math/Matrix.hpp
deleted file mode 100644
index f19db15ec..000000000
--- a/src/modules/mathlib/math/Matrix.hpp
+++ /dev/null
@@ -1,61 +0,0 @@
-/****************************************************************************
- *
- *   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 Matrix.h
- *
- * matrix code
- */
-
-#pragma once
-
-#include <nuttx/config.h>
-
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-#include "arm/Matrix.hpp"
-#else
-#include "generic/Matrix.hpp"
-#endif
-
-namespace math
-{
-class Matrix;
-int matrixTest();
-int matrixAddTest();
-int matrixSubTest();
-int matrixMultTest();
-int matrixInvTest();
-int matrixDivTest();
-int matrixArmTest();
-bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
-} // namespace math
diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/modules/mathlib/math/Quaternion.cpp
deleted file mode 100644
index 02fec4ca6..000000000
--- a/src/modules/mathlib/math/Quaternion.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-/****************************************************************************
- *
- *   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 Quaternion.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-
-#include "Quaternion.hpp"
-#include "Dcm.hpp"
-#include "EulerAngles.hpp"
-
-namespace math
-{
-
-Quaternion::Quaternion() :
-	Vector(4)
-{
-	setA(1.0f);
-	setB(0.0f);
-	setC(0.0f);
-	setD(0.0f);
-}
-
-Quaternion::Quaternion(float a, float b,
-		       float c, float d) :
-	Vector(4)
-{
-	setA(a);
-	setB(b);
-	setC(c);
-	setD(d);
-}
-
-Quaternion::Quaternion(const float *data) :
-	Vector(4, data)
-{
-}
-
-Quaternion::Quaternion(const Vector &v) :
-	Vector(v)
-{
-}
-
-Quaternion::Quaternion(const Dcm &dcm) :
-	Vector(4)
-{
-	// avoiding singularities by not using
-	// division equations
-	setA(0.5 * sqrt(1.0 +
-			double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
-	setB(0.5 * sqrt(1.0 +
-			double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
-	setC(0.5 * sqrt(1.0 +
-			double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
-	setD(0.5 * sqrt(1.0 +
-			double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
-}
-
-Quaternion::Quaternion(const EulerAngles &euler) :
-	Vector(4)
-{
-	double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
-	double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
-	double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
-	double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
-	double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
-	double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
-	setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
-	     sinPhi_2 * sinTheta_2 * sinPsi_2);
-	setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
-	     cosPhi_2 * sinTheta_2 * sinPsi_2);
-	setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
-	     sinPhi_2 * cosTheta_2 * sinPsi_2);
-	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
-	     sinPhi_2 * sinTheta_2 * cosPsi_2);
-}
-
-Quaternion::Quaternion(const Quaternion &right) :
-	Vector(right)
-{
-}
-
-Quaternion::~Quaternion()
-{
-}
-
-Vector Quaternion::derivative(const Vector &w)
-{
-#ifdef QUATERNION_ASSERT
-	ASSERT(w.getRows() == 3);
-#endif
-	float dataQ[] = {
-		getA(), -getB(), -getC(), -getD(),
-		getB(),  getA(), -getD(),  getC(),
-		getC(),  getD(),  getA(), -getB(),
-		getD(), -getC(),  getB(),  getA()
-	};
-	Vector v(4);
-	v(0) = 0.0f;
-	v(1) = w(0);
-	v(2) = w(1);
-	v(3) = w(2);
-	Matrix Q(4, 4, dataQ);
-	return Q * v * 0.5f;
-}
-
-int __EXPORT quaternionTest()
-{
-	printf("Test Quaternion\t\t: ");
-	// test default ctor
-	Quaternion q;
-	ASSERT(equal(q.getA(), 1.0f));
-	ASSERT(equal(q.getB(), 0.0f));
-	ASSERT(equal(q.getC(), 0.0f));
-	ASSERT(equal(q.getD(), 0.0f));
-	// test float ctor
-	q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
-	ASSERT(equal(q.getA(), 0.1825742f));
-	ASSERT(equal(q.getB(), 0.3651484f));
-	ASSERT(equal(q.getC(), 0.5477226f));
-	ASSERT(equal(q.getD(), 0.7302967f));
-	// test euler ctor
-	q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
-	// test dcm ctor
-	q = Quaternion(Dcm());
-	ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
-	// TODO test derivative
-	// test accessors
-	q.setA(0.1f);
-	q.setB(0.2f);
-	q.setC(0.3f);
-	q.setD(0.4f);
-	ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
-	printf("PASS\n");
-	return 0;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/modules/mathlib/math/Quaternion.hpp
deleted file mode 100644
index 048a55d33..000000000
--- a/src/modules/mathlib/math/Quaternion.hpp
+++ /dev/null
@@ -1,115 +0,0 @@
-/****************************************************************************
- *
- *   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 Quaternion.hpp
- *
- * math quaternion lib
- */
-
-#pragma once
-
-#include "Vector.hpp"
-#include "Matrix.hpp"
-
-namespace math
-{
-
-class Dcm;
-class EulerAngles;
-
-class __EXPORT Quaternion : public Vector
-{
-public:
-
-	/**
-	 * default ctor
-	 */
-	Quaternion();
-
-	/**
-	 * ctor from floats
-	 */
-	Quaternion(float a, float b, float c, float d);
-
-	/**
-	 * ctor from data
-	 */
-	Quaternion(const float *data);
-
-	/**
-	 * ctor from Vector
-	 */
-	Quaternion(const Vector &v);
-
-	/**
-	 * ctor from EulerAngles
-	 */
-	Quaternion(const EulerAngles &euler);
-
-	/**
-	 * ctor from Dcm
-	 */
-	Quaternion(const Dcm &dcm);
-
-	/**
-	 * deep copy ctor
-	 */
-	Quaternion(const Quaternion &right);
-
-	/**
-	 * dtor
-	 */
-	virtual ~Quaternion();
-
-	/**
-	 * derivative
-	 */
-	Vector derivative(const Vector &w);
-
-	/**
-	 * accessors
-	 */
-	void setA(float a) { (*this)(0) = a; }
-	void setB(float b) { (*this)(1) = b; }
-	void setC(float c) { (*this)(2) = c; }
-	void setD(float d) { (*this)(3) = d; }
-	const float &getA() const { return (*this)(0); }
-	const float &getB() const { return (*this)(1); }
-	const float &getC() const { return (*this)(2); }
-	const float &getD() const { return (*this)(3); }
-};
-
-int __EXPORT quaternionTest();
-} // math
-
diff --git a/src/modules/mathlib/math/Vector.cpp b/src/modules/mathlib/math/Vector.cpp
deleted file mode 100644
index 35158a396..000000000
--- a/src/modules/mathlib/math/Vector.cpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-#include "Vector.hpp"
-
-namespace math
-{
-
-static const float data_testA[] = {1, 3};
-static const float data_testB[] = {4, 1};
-
-static Vector testA(2, data_testA);
-static Vector testB(2, data_testB);
-
-int __EXPORT vectorTest()
-{
-	vectorAddTest();
-	vectorSubTest();
-	return 0;
-}
-
-int vectorAddTest()
-{
-	printf("Test Vector Add\t\t: ");
-	Vector r = testA + testB;
-	float data_test[] = {5.0f, 4.0f};
-	ASSERT(vectorEqual(Vector(2, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int vectorSubTest()
-{
-	printf("Test Vector Sub\t\t: ");
-	Vector r(2);
-	r = testA - testB;
-	float data_test[] = { -3.0f, 2.0f};
-	ASSERT(vectorEqual(Vector(2, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-bool vectorEqual(const Vector &a, const Vector &b, float eps)
-{
-	if (a.getRows() != b.getRows()) {
-		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
-		return false;
-	}
-
-	bool ret = true;
-
-	for (size_t i = 0; i < a.getRows(); i++) {
-		if (!equal(a(i), b(i), eps)) {
-			printf("element mismatch (%d)\n", i);
-			ret = false;
-		}
-	}
-
-	return ret;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/Vector.hpp b/src/modules/mathlib/math/Vector.hpp
deleted file mode 100644
index 73de793d5..000000000
--- a/src/modules/mathlib/math/Vector.hpp
+++ /dev/null
@@ -1,57 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector.h
- *
- * math vector
- */
-
-#pragma once
-
-#include <nuttx/config.h>
-
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-#include "arm/Vector.hpp"
-#else
-#include "generic/Vector.hpp"
-#endif
-
-namespace math
-{
-class Vector;
-int __EXPORT vectorTest();
-int __EXPORT vectorAddTest();
-int __EXPORT vectorSubTest();
-bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
-} // math
diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/modules/mathlib/math/Vector2f.cpp
deleted file mode 100644
index 68e741817..000000000
--- a/src/modules/mathlib/math/Vector2f.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector2f.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-#include "Vector2f.hpp"
-
-namespace math
-{
-
-Vector2f::Vector2f() :
-	Vector(2)
-{
-}
-
-Vector2f::Vector2f(const Vector &right) :
-	Vector(right)
-{
-#ifdef VECTOR_ASSERT
-	ASSERT(right.getRows() == 2);
-#endif
-}
-
-Vector2f::Vector2f(float x, float y) :
-	Vector(2)
-{
-	setX(x);
-	setY(y);
-}
-
-Vector2f::Vector2f(const float *data) :
-	Vector(2, data)
-{
-}
-
-Vector2f::~Vector2f()
-{
-}
-
-float Vector2f::cross(const Vector2f &b) const
-{
-	const Vector2f &a = *this;
-	return a(0)*b(1) - a(1)*b(0);
-}
-
-float Vector2f::operator %(const Vector2f &v) const
-{
-	return cross(v);
-}
-    
-float Vector2f::operator *(const Vector2f &v) const
-{
-    return dot(v);
-}
-
-int __EXPORT vector2fTest()
-{
-	printf("Test Vector2f\t\t: ");
-	// test float ctor
-	Vector2f v(1, 2);
-	ASSERT(equal(v(0), 1));
-	ASSERT(equal(v(1), 2));
-	printf("PASS\n");
-	return 0;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/modules/mathlib/math/Vector2f.hpp
deleted file mode 100644
index ecd62e81c..000000000
--- a/src/modules/mathlib/math/Vector2f.hpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector2f.hpp
- *
- * math 3 vector
- */
-
-#pragma once
-
-#include "Vector.hpp"
-
-namespace math
-{
-
-class __EXPORT Vector2f :
-	public Vector
-{
-public:
-	Vector2f();
-	Vector2f(const Vector &right);
-	Vector2f(float x, float y);
-	Vector2f(const float *data);
-	virtual ~Vector2f();
-	float cross(const Vector2f &b) const;
-	float operator %(const Vector2f &v) const;
-    float operator *(const Vector2f &v) const;
-    inline Vector2f operator*(const float &right) const {
-		return Vector::operator*(right);
-	}
-
-	/**
-	 * accessors
-	 */
-	void setX(float x) { (*this)(0) = x; }
-	void setY(float y) { (*this)(1) = y; }
-	const float &getX() const { return (*this)(0); }
-	const float &getY() const { return (*this)(1); }
-};
-    
-class __EXPORT Vector2 :
-	public Vector2f
-{
-};
-
-int __EXPORT vector2fTest();
-} // math
-
diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp
deleted file mode 100644
index dcb85600e..000000000
--- a/src/modules/mathlib/math/Vector3.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector3.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-#include "Vector3.hpp"
-
-namespace math
-{
-
-Vector3::Vector3() :
-	Vector(3)
-{
-}
-
-Vector3::Vector3(const Vector &right) :
-	Vector(right)
-{
-#ifdef VECTOR_ASSERT
-	ASSERT(right.getRows() == 3);
-#endif
-}
-
-Vector3::Vector3(float x, float y, float z) :
-	Vector(3)
-{
-	setX(x);
-	setY(y);
-	setZ(z);
-}
-
-Vector3::Vector3(const float *data) :
-	Vector(3, data)
-{
-}
-
-Vector3::~Vector3()
-{
-}
-
-Vector3 Vector3::cross(const Vector3 &b) const
-{
-	const Vector3 &a = *this;
-	Vector3 result;
-	result(0) = a(1) * b(2) - a(2) * b(1);
-	result(1) = a(2) * b(0) - a(0) * b(2);
-	result(2) = a(0) * b(1) - a(1) * b(0);
-	return result;
-}
-
-int __EXPORT vector3Test()
-{
-	printf("Test Vector3\t\t: ");
-	// test float ctor
-	Vector3 v(1, 2, 3);
-	ASSERT(equal(v(0), 1));
-	ASSERT(equal(v(1), 2));
-	ASSERT(equal(v(2), 3));
-	printf("PASS\n");
-	return 0;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp
deleted file mode 100644
index 568d9669a..000000000
--- a/src/modules/mathlib/math/Vector3.hpp
+++ /dev/null
@@ -1,76 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector3.hpp
- *
- * math 3 vector
- */
-
-#pragma once
-
-#include "Vector.hpp"
-
-namespace math
-{
-
-class __EXPORT Vector3 :
-	public Vector
-{
-public:
-	Vector3();
-	Vector3(const Vector &right);
-	Vector3(float x, float y, float z);
-	Vector3(const float *data);
-	virtual ~Vector3();
-	Vector3 cross(const Vector3 &b) const;
-
-	/**
-	 * accessors
-	 */
-	void setX(float x) { (*this)(0) = x; }
-	void setY(float y) { (*this)(1) = y; }
-	void setZ(float z) { (*this)(2) = z; }
-	const float &getX() const { return (*this)(0); }
-	const float &getY() const { return (*this)(1); }
-	const float &getZ() const { return (*this)(2); }
-};
-    
-class __EXPORT Vector3f :
-	public Vector3
-{
-};
-
-int __EXPORT vector3Test();
-} // math
-
diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/modules/mathlib/math/arm/Matrix.cpp
deleted file mode 100644
index 21661622a..000000000
--- a/src/modules/mathlib/math/arm/Matrix.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   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 Matrix.cpp
- *
- * matrix code
- */
-
-#include "Matrix.hpp"
diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/modules/mathlib/math/arm/Matrix.hpp
deleted file mode 100644
index 715fd3a5e..000000000
--- a/src/modules/mathlib/math/arm/Matrix.hpp
+++ /dev/null
@@ -1,292 +0,0 @@
-/****************************************************************************
- *
- *   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 Matrix.h
- *
- * matrix code
- */
-
-#pragma once
-
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-#include "../Matrix.hpp"
-
-// arm specific
-#include "../../CMSIS/Include/arm_math.h"
-
-namespace math
-{
-
-class __EXPORT Matrix
-{
-public:
-	// constructor
-	Matrix(size_t rows, size_t cols) :
-		_matrix() {
-		arm_mat_init_f32(&_matrix,
-				 rows, cols,
-				 (float *)calloc(rows * cols, sizeof(float)));
-	}
-	Matrix(size_t rows, size_t cols, const float *data) :
-		_matrix() {
-		arm_mat_init_f32(&_matrix,
-				 rows, cols,
-				 (float *)malloc(rows * cols * sizeof(float)));
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Matrix() {
-		delete [] _matrix.pData;
-	}
-	// copy constructor (deep)
-	Matrix(const Matrix &right) :
-		_matrix() {
-		arm_mat_init_f32(&_matrix,
-				 right.getRows(), right.getCols(),
-				 (float *)malloc(right.getRows()*
-						 right.getCols()*sizeof(float)));
-		memcpy(getData(), right.getData(),
-		       getSize());
-	}
-	// assignment
-	inline Matrix &operator=(const Matrix &right) {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i, size_t j) {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	inline const float &operator()(size_t i, size_t j) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				float sig;
-				int exp;
-				float num = (*this)(i, j);
-				float2SigExp(num, sig, exp);
-				printf("%6.3fe%03.3d,", (double)sig, exp);
-			}
-
-			printf("\n");
-		}
-	}
-	// boolean ops
-	inline bool operator==(const Matrix &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
-					return false;
-			}
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Matrix operator+(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_offset_f32((float *)getData(), right,
-			       (float *)result.getData(), getRows()*getCols());
-		return result;
-	}
-	inline Matrix operator-(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_offset_f32((float *)getData(), -right,
-			       (float *)result.getData(), getRows()*getCols());
-		return result;
-	}
-	inline Matrix operator*(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_mat_scale_f32(&_matrix, right,
-				  &(result._matrix));
-		return result;
-	}
-	inline Matrix operator/(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_mat_scale_f32(&_matrix, 1.0f / right,
-				  &(result._matrix));
-		return result;
-	}
-	// vector ops
-	inline Vector operator*(const Vector &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Matrix resultMat = (*this) *
-				   Matrix(right.getRows(), 1, right.getData());
-		return Vector(getRows(), resultMat.getData());
-	}
-	// matrix ops
-	inline Matrix operator+(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-		arm_mat_add_f32(&_matrix, &(right._matrix),
-				&(result._matrix));
-		return result;
-	}
-	inline Matrix operator-(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-		arm_mat_sub_f32(&_matrix, &(right._matrix),
-				&(result._matrix));
-		return result;
-	}
-	inline Matrix operator*(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Matrix result(getRows(), right.getCols());
-		arm_mat_mult_f32(&_matrix, &(right._matrix),
-				 &(result._matrix));
-		return result;
-	}
-	inline Matrix operator/(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(right.getRows() == right.getCols());
-		ASSERT(getCols() == right.getCols());
-#endif
-		return (*this) * right.inverse();
-	}
-	// other functions
-	inline Matrix transpose() const {
-		Matrix result(getCols(), getRows());
-		arm_mat_trans_f32(&_matrix, &(result._matrix));
-		return result;
-	}
-	inline void swapRows(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t j = 0; j < getCols(); j++) {
-			float tmp = (*this)(a, j);
-			(*this)(a, j) = (*this)(b, j);
-			(*this)(b, j) = tmp;
-		}
-	}
-	inline void swapCols(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t i = 0; i < getRows(); i++) {
-			float tmp = (*this)(i, a);
-			(*this)(i, a) = (*this)(i, b);
-			(*this)(i, b) = tmp;
-		}
-	}
-	/**
-	 * inverse based on LU factorization with partial pivotting
-	 */
-	Matrix inverse() const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == getCols());
-#endif
-		Matrix result(getRows(), getCols());
-		Matrix work = (*this);
-		arm_mat_inverse_f32(&(work._matrix),
-				    &(result._matrix));
-		return result;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				(*this)(i, j) = val;
-			}
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _matrix.numRows; }
-	inline size_t getCols() const { return _matrix.numCols; }
-	inline static Matrix identity(size_t size) {
-		Matrix result(size, size);
-
-		for (size_t i = 0; i < size; i++) {
-			result(i, i) = 1.0f;
-		}
-
-		return result;
-	}
-	inline static Matrix zero(size_t size) {
-		Matrix result(size, size);
-		result.setAll(0.0f);
-		return result;
-	}
-	inline static Matrix zero(size_t m, size_t n) {
-		Matrix result(m, n);
-		result.setAll(0.0f);
-		return result;
-	}
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
-	inline float *getData() { return _matrix.pData; }
-	inline const float *getData() const { return _matrix.pData; }
-	inline void setData(float *data) { _matrix.pData = data; }
-private:
-	arm_matrix_instance_f32 _matrix;
-};
-
-} // namespace math
diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/modules/mathlib/math/arm/Vector.cpp
deleted file mode 100644
index 7ea6496bb..000000000
--- a/src/modules/mathlib/math/arm/Vector.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector.cpp
- *
- * math vector
- */
-
-#include "Vector.hpp"
diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp
deleted file mode 100644
index 4155800e8..000000000
--- a/src/modules/mathlib/math/arm/Vector.hpp
+++ /dev/null
@@ -1,236 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector.h
- *
- * math vector
- */
-
-#pragma once
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-#include "../test/test.hpp"
-
-// arm specific
-#include "../../CMSIS/Include/arm_math.h"
-
-namespace math
-{
-
-class __EXPORT Vector
-{
-public:
-	// constructor
-	Vector(size_t rows) :
-		_rows(rows),
-		_data((float *)calloc(rows, sizeof(float))) {
-	}
-	Vector(size_t rows, const float *data) :
-		_rows(rows),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Vector() {
-		delete [] getData();
-	}
-	// copy constructor (deep)
-	Vector(const Vector &right) :
-		_rows(right.getRows()),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), right.getData(),
-		       right.getSize());
-	}
-	// assignment
-	inline Vector &operator=(const Vector &right) {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i) {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	inline const float &operator()(size_t i) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			float sig;
-			int exp;
-			float num = (*this)(i);
-			float2SigExp(num, sig, exp);
-			printf("%6.3fe%03.3d,", (double)sig, exp);
-		}
-
-		printf("\n");
-	}
-	// boolean ops
-	inline bool operator==(const Vector &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
-				return false;
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Vector operator+(float right) const {
-		Vector result(getRows());
-		arm_offset_f32((float *)getData(),
-			       right, result.getData(),
-			       getRows());
-		return result;
-	}
-	inline Vector operator-(float right) const {
-		Vector result(getRows());
-		arm_offset_f32((float *)getData(),
-			       -right, result.getData(),
-			       getRows());
-		return result;
-	}
-	inline Vector operator*(float right) const {
-		Vector result(getRows());
-		arm_scale_f32((float *)getData(),
-			      right, result.getData(),
-			      getRows());
-		return result;
-	}
-	inline Vector operator/(float right) const {
-		Vector result(getRows());
-		arm_scale_f32((float *)getData(),
-			      1.0f / right, result.getData(),
-			      getRows());
-		return result;
-	}
-	// vector ops
-	inline Vector operator+(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-		arm_add_f32((float *)getData(),
-			    (float *)right.getData(),
-			    result.getData(),
-			    getRows());
-		return result;
-	}
-	inline Vector operator-(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-		arm_sub_f32((float *)getData(),
-			    (float *)right.getData(),
-			    result.getData(),
-			    getRows());
-		return result;
-	}
-	inline Vector operator-(void) const {
-		Vector result(getRows());
-		arm_negate_f32((float *)getData(),
-			    result.getData(),
-			    getRows());
-		return result;
-	}
-	// other functions
-	inline float dot(const Vector &right) const {
-		float result = 0;
-		arm_dot_prod_f32((float *)getData(),
-				 (float *)right.getData(),
-				 getRows(),
-				 &result);
-		return result;
-	}
-	inline float norm() const {
-		return sqrtf(dot(*this));
-	}
-	inline float length() const {
-		return norm();
-	}
-	inline Vector unit() const {
-		return (*this) / norm();
-	}
-	inline Vector normalized() const {
-		return unit();
-	}
-	inline void normalize() {
-		(*this) = (*this) / norm();
-	}
-	inline static Vector zero(size_t rows) {
-		Vector result(rows);
-		// calloc returns zeroed memory
-		return result;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			(*this)(i) = val;
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _rows; }
-	inline const float *getData() const { return _data; }
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows(); }
-	inline float *getData() { return _data; }
-	inline void setData(float *data) { _data = data; }
-private:
-	size_t _rows;
-	float *_data;
-};
-
-} // math
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp
deleted file mode 100644
index efb17225d..000000000
--- a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-/****************************************************************************
- *
- *   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	LowPassFilter.cpp
-/// @brief	A class to implement a second order low pass filter 
-/// Author: Leonard Hall <LeonardTHall@gmail.com>
-
-#include "LowPassFilter2p.hpp"
-#include "math.h"
-
-namespace math
-{
-
-void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
-{
-    _cutoff_freq = cutoff_freq;
-    float fr = sample_freq/_cutoff_freq;
-    float ohm = tanf(M_PI_F/fr);
-    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
-    _b0 = ohm*ohm/c;
-    _b1 = 2.0f*_b0;
-    _b2 = _b0;
-    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
-    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
-}
-
-float LowPassFilter2p::apply(float sample)
-{
-    // do the filtering
-    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
-    if (isnan(delay_element_0) || isinf(delay_element_0)) {
-        // don't allow bad values to propogate via the filter
-        delay_element_0 = sample;
-    }
-    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
-    
-    _delay_element_2 = _delay_element_1;
-    _delay_element_1 = delay_element_0;
-
-    // return the value.  Should be no need to check limits
-    return output;
-}
-
-} // namespace math
-
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp
deleted file mode 100644
index 208ec98d4..000000000
--- a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp
+++ /dev/null
@@ -1,78 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-/****************************************************************************
- *
- *   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	LowPassFilter.h
-/// @brief	A class to implement a second order low pass filter 
-/// Author: Leonard Hall <LeonardTHall@gmail.com>
-/// Adapted for PX4 by Andrew Tridgell
-
-#pragma once
-
-namespace math
-{
-class __EXPORT LowPassFilter2p
-{
-public:
-    // constructor
-    LowPassFilter2p(float sample_freq, float cutoff_freq) {
-        // set initial parameters
-        set_cutoff_frequency(sample_freq, cutoff_freq);
-        _delay_element_1 = _delay_element_2 = 0;
-    }
-
-    // change parameters
-    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
-
-    // apply - Add a new raw value to the filter 
-    // and retrieve the filtered result
-    float apply(float sample);
-
-    // return the cutoff frequency
-    float get_cutoff_freq(void) const {
-        return _cutoff_freq;
-    }
-
-private:
-    float           _cutoff_freq; 
-    float           _a1;
-    float           _a2;
-    float           _b0;
-    float           _b1;
-    float           _b2;
-    float           _delay_element_1;        // buffered sample -1
-    float           _delay_element_2;        // buffered sample -2
-};
-
-} // namespace math
diff --git a/src/modules/mathlib/math/filter/module.mk b/src/modules/mathlib/math/filter/module.mk
deleted file mode 100644
index fe92c8c70..000000000
--- a/src/modules/mathlib/math/filter/module.mk
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# filter library
-#
-SRCS		 = LowPassFilter2p.cpp
-
-#
-# In order to include .config we first have to save off the
-# current makefile name, since app.mk needs it.
-#
-APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
--include $(TOPDIR)/.config
diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/modules/mathlib/math/generic/Matrix.cpp
deleted file mode 100644
index 21661622a..000000000
--- a/src/modules/mathlib/math/generic/Matrix.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   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 Matrix.cpp
- *
- * matrix code
- */
-
-#include "Matrix.hpp"
diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/modules/mathlib/math/generic/Matrix.hpp
deleted file mode 100644
index 5601a3447..000000000
--- a/src/modules/mathlib/math/generic/Matrix.hpp
+++ /dev/null
@@ -1,437 +0,0 @@
-/****************************************************************************
- *
- *   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 Matrix.h
- *
- * matrix code
- */
-
-#pragma once
-
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-#include "../Matrix.hpp"
-
-namespace math
-{
-
-class __EXPORT Matrix
-{
-public:
-	// constructor
-	Matrix(size_t rows, size_t cols) :
-		_rows(rows),
-		_cols(cols),
-		_data((float *)calloc(rows *cols, sizeof(float))) {
-	}
-	Matrix(size_t rows, size_t cols, const float *data) :
-		_rows(rows),
-		_cols(cols),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Matrix() {
-		delete [] getData();
-	}
-	// copy constructor (deep)
-	Matrix(const Matrix &right) :
-		_rows(right.getRows()),
-		_cols(right.getCols()),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), right.getData(),
-		       right.getSize());
-	}
-	// assignment
-	inline Matrix &operator=(const Matrix &right) {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i, size_t j) {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	inline const float &operator()(size_t i, size_t j) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				float sig;
-				int exp;
-				float num = (*this)(i, j);
-				float2SigExp(num, sig, exp);
-				printf("%6.3fe%03.3d,", (double)sig, exp);
-			}
-
-			printf("\n");
-		}
-	}
-	// boolean ops
-	inline bool operator==(const Matrix &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
-					return false;
-			}
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Matrix operator+(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) + right;
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator-(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) - right;
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator*(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) * right;
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator/(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) / right;
-			}
-		}
-
-		return result;
-	}
-	// vector ops
-	inline Vector operator*(const Vector &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i) += (*this)(i, j) * right(j);
-			}
-		}
-
-		return result;
-	}
-	// matrix ops
-	inline Matrix operator+(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) + right(i, j);
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator-(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) - right(i, j);
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator*(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Matrix result(getRows(), right.getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < right.getCols(); j++) {
-				for (size_t k = 0; k < right.getRows(); k++) {
-					result(i, j) += (*this)(i, k) * right(k, j);
-				}
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator/(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(right.getRows() == right.getCols());
-		ASSERT(getCols() == right.getCols());
-#endif
-		return (*this) * right.inverse();
-	}
-	// other functions
-	inline Matrix transpose() const {
-		Matrix result(getCols(), getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(j, i) = (*this)(i, j);
-			}
-		}
-
-		return result;
-	}
-	inline void swapRows(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t j = 0; j < getCols(); j++) {
-			float tmp = (*this)(a, j);
-			(*this)(a, j) = (*this)(b, j);
-			(*this)(b, j) = tmp;
-		}
-	}
-	inline void swapCols(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t i = 0; i < getRows(); i++) {
-			float tmp = (*this)(i, a);
-			(*this)(i, a) = (*this)(i, b);
-			(*this)(i, b) = tmp;
-		}
-	}
-	/**
-	 * inverse based on LU factorization with partial pivotting
-	 */
-	Matrix inverse() const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == getCols());
-#endif
-		size_t N = getRows();
-		Matrix L = identity(N);
-		const Matrix &A = (*this);
-		Matrix U = A;
-		Matrix P = identity(N);
-
-		//printf("A:\n"); A.print();
-
-		// for all diagonal elements
-		for (size_t n = 0; n < N; n++) {
-
-			// if diagonal is zero, swap with row below
-			if (fabsf(U(n, n)) < 1e-8f) {
-				//printf("trying pivot for row %d\n",n);
-				for (size_t i = 0; i < N; i++) {
-					if (i == n) continue;
-
-					//printf("\ttrying row %d\n",i);
-					if (fabsf(U(i, n)) > 1e-8f) {
-						//printf("swapped %d\n",i);
-						U.swapRows(i, n);
-						P.swapRows(i, n);
-					}
-				}
-			}
-
-#ifdef MATRIX_ASSERT
-			//printf("A:\n"); A.print();
-			//printf("U:\n"); U.print();
-			//printf("P:\n"); P.print();
-			//fflush(stdout);
-			ASSERT(fabsf(U(n, n)) > 1e-8f);
-#endif
-
-			// failsafe, return zero matrix
-			if (fabsf(U(n, n)) < 1e-8f) {
-				return Matrix::zero(n);
-			}
-
-			// for all rows below diagonal
-			for (size_t i = (n + 1); i < N; i++) {
-				L(i, n) = U(i, n) / U(n, n);
-
-				// add i-th row and n-th row
-				// multiplied by: -a(i,n)/a(n,n)
-				for (size_t k = n; k < N; k++) {
-					U(i, k) -= L(i, n) * U(n, k);
-				}
-			}
-		}
-
-		//printf("L:\n"); L.print();
-		//printf("U:\n"); U.print();
-
-		// solve LY=P*I for Y by forward subst
-		Matrix Y = P;
-
-		// for all columns of Y
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of L
-			for (size_t i = 0; i < N; i++) {
-				// for all columns of L
-				for (size_t j = 0; j < i; j++) {
-					// for all existing y
-					// subtract the component they
-					// contribute to the solution
-					Y(i, c) -= L(i, j) * Y(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				// Y(i,c) /= L(i,i);
-				// but L(i,i) = 1.0
-			}
-		}
-
-		//printf("Y:\n"); Y.print();
-
-		// solve Ux=y for x by back subst
-		Matrix X = Y;
-
-		// for all columns of X
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of U
-			for (size_t k = 0; k < N; k++) {
-				// have to go in reverse order
-				size_t i = N - 1 - k;
-
-				// for all columns of U
-				for (size_t j = i + 1; j < N; j++) {
-					// for all existing x
-					// subtract the component they
-					// contribute to the solution
-					X(i, c) -= U(i, j) * X(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				X(i, c) /= U(i, i);
-			}
-		}
-
-		//printf("X:\n"); X.print();
-		return X;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				(*this)(i, j) = val;
-			}
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _rows; }
-	inline size_t getCols() const { return _cols; }
-	inline static Matrix identity(size_t size) {
-		Matrix result(size, size);
-
-		for (size_t i = 0; i < size; i++) {
-			result(i, i) = 1.0f;
-		}
-
-		return result;
-	}
-	inline static Matrix zero(size_t size) {
-		Matrix result(size, size);
-		result.setAll(0.0f);
-		return result;
-	}
-	inline static Matrix zero(size_t m, size_t n) {
-		Matrix result(m, n);
-		result.setAll(0.0f);
-		return result;
-	}
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
-	inline float *getData() { return _data; }
-	inline const float *getData() const { return _data; }
-	inline void setData(float *data) { _data = data; }
-private:
-	size_t _rows;
-	size_t _cols;
-	float *_data;
-};
-
-} // namespace math
diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/modules/mathlib/math/generic/Vector.cpp
deleted file mode 100644
index 7ea6496bb..000000000
--- a/src/modules/mathlib/math/generic/Vector.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector.cpp
- *
- * math vector
- */
-
-#include "Vector.hpp"
diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp
deleted file mode 100644
index 8cfdc676d..000000000
--- a/src/modules/mathlib/math/generic/Vector.hpp
+++ /dev/null
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- *   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 Vector.h
- *
- * math vector
- */
-
-#pragma once
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-
-namespace math
-{
-
-class __EXPORT Vector
-{
-public:
-	// constructor
-	Vector(size_t rows) :
-		_rows(rows),
-		_data((float *)calloc(rows, sizeof(float))) {
-	}
-	Vector(size_t rows, const float *data) :
-		_rows(rows),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Vector() {
-		delete [] getData();
-	}
-	// copy constructor (deep)
-	Vector(const Vector &right) :
-		_rows(right.getRows()),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), right.getData(),
-		       right.getSize());
-	}
-	// assignment
-	inline Vector &operator=(const Vector &right) {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i) {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	inline const float &operator()(size_t i) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			float sig;
-			int exp;
-			float num = (*this)(i);
-			float2SigExp(num, sig, exp);
-			printf("%6.3fe%03.3d,", (double)sig, exp);
-		}
-
-		printf("\n");
-	}
-	// boolean ops
-	inline bool operator==(const Vector &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
-				return false;
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Vector operator+(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) + right;
-		}
-
-		return result;
-	}
-	inline Vector operator-(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) - right;
-		}
-
-		return result;
-	}
-	inline Vector operator*(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) * right;
-		}
-
-		return result;
-	}
-	inline Vector operator/(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) / right;
-		}
-
-		return result;
-	}
-	// vector ops
-	inline Vector operator+(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) + right(i);
-		}
-
-		return result;
-	}
-	inline Vector operator-(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) - right(i);
-		}
-
-		return result;
-	}
-	inline Vector operator-(void) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = -((*this)(i));
-		}
-
-		return result;
-	}
-	// other functions
-	inline float dot(const Vector &right) const {
-		float result = 0;
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result += (*this)(i) * (*this)(i);
-		}
-
-		return result;
-	}
-	inline float norm() const {
-		return sqrtf(dot(*this));
-	}
-	inline float length() const {
-		return norm();
-	}
-	inline Vector unit() const {
-		return (*this) / norm();
-	}
-	inline Vector normalized() const {
-		return unit();
-	}
-	inline void normalize() {
-		(*this) = (*this) / norm();
-	}
-	inline static Vector zero(size_t rows) {
-		Vector result(rows);
-		// calloc returns zeroed memory
-		return result;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			(*this)(i) = val;
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _rows; }
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows(); }
-	inline float *getData() { return _data; }
-	inline const float *getData() const { return _data; }
-	inline void setData(float *data) { _data = data; }
-private:
-	size_t _rows;
-	float *_data;
-};
-
-} // math
diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/modules/mathlib/math/nasa_rotation_def.pdf
deleted file mode 100644
index eb67a4bfc..000000000
Binary files a/src/modules/mathlib/math/nasa_rotation_def.pdf and /dev/null differ
diff --git a/src/modules/mathlib/math/test/test.cpp b/src/modules/mathlib/math/test/test.cpp
deleted file mode 100644
index 2fa2f7e7c..000000000
--- a/src/modules/mathlib/math/test/test.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/****************************************************************************
- *
- *   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 test.cpp
- *
- * Test library code
- */
-
-#include <stdio.h>
-#include <math.h>
-#include <stdlib.h>
-
-#include "test.hpp"
-
-bool __EXPORT equal(float a, float b, float epsilon)
-{
-	float diff = fabsf(a - b);
-
-	if (diff > epsilon) {
-		printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
-		return false;
-
-	} else return true;
-}
-
-void __EXPORT float2SigExp(
-	const float &num,
-	float &sig,
-	int &exp)
-{
-	if (isnan(num) || isinf(num)) {
-		sig = 0.0f;
-		exp = -99;
-		return;
-	}
-
-	if (fabsf(num) < 1.0e-38f) {
-		sig = 0;
-		exp = 0;
-		return;
-	}
-
-	exp = log10f(fabsf(num));
-
-	if (exp > 0) {
-		exp = ceil(exp);
-
-	} else {
-		exp = floor(exp);
-	}
-
-	sig = num;
-
-	// cheap power since it is integer
-	if (exp > 0) {
-		for (int i = 0; i < abs(exp); i++) sig /= 10;
-
-	} else {
-		for (int i = 0; i < abs(exp); i++) sig *= 10;
-	}
-}
-
-
diff --git a/src/modules/mathlib/math/test/test.hpp b/src/modules/mathlib/math/test/test.hpp
deleted file mode 100644
index 2027bb827..000000000
--- a/src/modules/mathlib/math/test/test.hpp
+++ /dev/null
@@ -1,50 +0,0 @@
-/****************************************************************************
- *
- *   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 test.hpp
- *
- * Controller library code
- */
-
-#pragma once
-
-//#include <assert.h>
-//#include <time.h>
-//#include <stdlib.h>
-
-bool equal(float a, float b, float eps = 1e-5);
-void float2SigExp(
-	const float &num,
-	float &sig,
-	int &exp);
diff --git a/src/modules/mathlib/math/test_math.sce b/src/modules/mathlib/math/test_math.sce
deleted file mode 100644
index c3fba4729..000000000
--- a/src/modules/mathlib/math/test_math.sce
+++ /dev/null
@@ -1,63 +0,0 @@
-clc
-clear
-function out = float_truncate(in, digits)
-    out = round(in*10^digits)
-    out = out/10^digits
-endfunction
-
-phi = 0.1
-theta = 0.2
-psi = 0.3
-
-cosPhi = cos(phi)
-cosPhi_2 = cos(phi/2)
-sinPhi = sin(phi)
-sinPhi_2 = sin(phi/2)
-
-cosTheta = cos(theta)
-cosTheta_2 = cos(theta/2)
-sinTheta = sin(theta)
-sinTheta_2 = sin(theta/2)
-
-cosPsi = cos(psi)
-cosPsi_2 = cos(psi/2)
-sinPsi = sin(psi)
-sinPsi_2 = sin(psi/2)
-
-C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
-        cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
-        -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
-    
-disp(C_nb)
-//C_nb = float_truncate(C_nb,3)
-//disp(C_nb)
-
-theta = asin(-C_nb(3,1))
-phi = atan(C_nb(3,2), C_nb(3,3))
-psi = atan(C_nb(2,1), C_nb(1,1))
-printf('phi %f\n', phi)
-printf('theta %f\n', theta)
-printf('psi %f\n', psi)
-
-q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
-     sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
-     cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
-     cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
-     
-//q = float_truncate(q,3)
-
-a = q(1)
-b = q(2)
-c = q(3)
-d = q(4)
-printf('q: %f %f %f %f\n', a, b, c, d)
-a2 = a*a
-b2 = b*b
-c2 = c*c
-d2 = d*d
-
-C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
-         2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
-         2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
-
-disp(C2_nb)
diff --git a/src/modules/mathlib/mathlib.h b/src/modules/mathlib/mathlib.h
deleted file mode 100644
index 40ffb22bc..000000000
--- a/src/modules/mathlib/mathlib.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/****************************************************************************
- *
- *   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 mathlib.h
- *
- * Common header for mathlib exports.
- */
-
-#ifdef __cplusplus
-
-#pragma once
-
-#include "math/Dcm.hpp"
-#include "math/EulerAngles.hpp"
-#include "math/Matrix.hpp"
-#include "math/Quaternion.hpp"
-#include "math/Vector.hpp"
-#include "math/Vector3.hpp"
-#include "math/Vector2f.hpp"
-#include "math/Limits.hpp"
-
-#endif
-
-#ifdef CONFIG_ARCH_ARM
-
-#include "CMSIS/Include/arm_math.h"
-
-#endif
\ No newline at end of file
diff --git a/src/modules/mathlib/module.mk b/src/modules/mathlib/module.mk
deleted file mode 100644
index 2146a1413..000000000
--- a/src/modules/mathlib/module.mk
+++ /dev/null
@@ -1,62 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Math library
-#
-SRCS		 = math/test/test.cpp \
-		   math/Vector.cpp \
-		   math/Vector2f.cpp \
-		   math/Vector3.cpp \
-		   math/EulerAngles.cpp \
-		   math/Quaternion.cpp \
-		   math/Dcm.cpp \
-		   math/Matrix.cpp \
-		   math/Limits.cpp
-
-#
-# In order to include .config we first have to save off the
-# current makefile name, since app.mk needs it.
-#
-APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
--include $(TOPDIR)/.config
-
-ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
-INCLUDE_DIRS	+= math/arm
-SRCS		+= math/arm/Vector.cpp \
-		   math/arm/Matrix.cpp
-else
-#INCLUDE_DIRS	+= math/generic
-#SRCS		+= math/generic/Vector.cpp \
-#		   math/generic/Matrix.cpp
-endif
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 3d3434670..f39bbaa72 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -279,6 +279,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
 		orb_set_interval(subs->act_2_sub, min_interval);
 		orb_set_interval(subs->act_3_sub, min_interval);
 		orb_set_interval(subs->actuators_sub, min_interval);
+		orb_set_interval(subs->actuators_effective_sub, min_interval);
+		orb_set_interval(subs->spa_sub, min_interval);
+		orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
 		break;
 
 	case MAVLINK_MSG_ID_MANUAL_CONTROL:
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 01bbabd46..28f7af33c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -50,6 +50,10 @@
 #include <mqueue.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -101,6 +105,10 @@ static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t pub_hil_attitude = -1;
 static orb_advert_t pub_hil_gps = -1;
 static orb_advert_t pub_hil_sensors = -1;
+static orb_advert_t pub_hil_gyro = -1;
+static orb_advert_t pub_hil_accel = -1;
+static orb_advert_t pub_hil_mag = -1;
+static orb_advert_t pub_hil_baro = -1;
 static orb_advert_t pub_hil_airspeed = -1;
 
 static orb_advert_t cmd_pub = -1;
@@ -412,12 +420,12 @@ handle_message(mavlink_message_t *msg)
 
 			/* airspeed from differential pressure, ambient pressure and temp */
 			struct airspeed_s airspeed;
-			airspeed.timestamp = hrt_absolute_time();
 
 			float ias = calc_indicated_airspeed(imu.diff_pressure);
 			// XXX need to fix this
 			float tas = ias;
 
+			airspeed.timestamp = hrt_absolute_time();
 			airspeed.indicated_airspeed_m_s = ias;
 			airspeed.true_airspeed_m_s = tas;
 
@@ -428,7 +436,67 @@ handle_message(mavlink_message_t *msg)
 			}
 			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
 
-			/* publish */
+			/* individual sensor publications */
+			struct gyro_report gyro;
+			gyro.x_raw = imu.xgyro / mrad2rad;
+			gyro.y_raw = imu.ygyro / mrad2rad;
+			gyro.z_raw = imu.zgyro / mrad2rad;
+			gyro.x = imu.xgyro;
+			gyro.y = imu.ygyro;
+			gyro.z = imu.zgyro;
+			gyro.temperature = imu.temperature;
+			gyro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_gyro < 0) {
+				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+			} else {
+				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
+			}
+
+			struct accel_report accel;
+			accel.x_raw = imu.xacc / mg2ms2;
+			accel.y_raw = imu.yacc / mg2ms2;
+			accel.z_raw = imu.zacc / mg2ms2;
+			accel.x = imu.xacc;
+			accel.y = imu.yacc;
+			accel.z = imu.zacc;
+			accel.temperature = imu.temperature;
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
+
+			struct mag_report mag;
+			mag.x_raw = imu.xmag / mga2ga;
+			mag.y_raw = imu.ymag / mga2ga;
+			mag.z_raw = imu.zmag / mga2ga;
+			mag.x = imu.xmag;
+			mag.y = imu.ymag;
+			mag.z = imu.zmag;
+			mag.timestamp = hrt_absolute_time();
+
+			if (pub_hil_mag < 0) {
+				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+			} else {
+				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
+			}
+
+			struct baro_report baro;
+			baro.pressure = imu.abs_pressure;
+			baro.altitude = imu.pressure_alt;
+			baro.temperature = imu.temperature;
+			baro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_baro < 0) {
+				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+			} else {
+				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
+			}
+
+			/* publish combined sensor topic */
 			if (pub_hil_sensors > 0) {
 				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
 			} else {
@@ -552,6 +620,22 @@ handle_message(mavlink_message_t *msg)
 			} else {
 				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 			}
+
+			struct accel_report accel;
+			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+			accel.x = hil_state.xacc;
+			accel.y = hil_state.yacc;
+			accel.z = hil_state.zacc;
+			accel.temperature = 25.0f;
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 2a260861d..c711b1803 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -71,7 +71,8 @@ struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_0;
+struct actuator_controls_effective_s actuators_effective_0;
+struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
 
 struct mavlink_subscriptions mavlink_subs;
@@ -112,6 +113,7 @@ static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
 static void	l_actuator_armed(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
+static void	l_vehicle_attitude_controls_effective(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
@@ -138,6 +140,7 @@ static const struct listener listeners[] = {
 	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
+	{l_vehicle_attitude_controls_effective,	&mavlink_subs.actuators_effective_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
@@ -567,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l)
 }
 
 void
-l_vehicle_attitude_controls(const struct listener *l)
+l_vehicle_attitude_controls_effective(const struct listener *l)
 {
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
 
 	if (gcs_link) {
 		/* send, add spaces so that string buffer is at least 10 chars long */
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl0    ",
-						   actuators_0.control_effective[0]);
+						   actuators_effective_0.control_effective[0]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl1    ",
-						   actuators_0.control_effective[1]);
+						   actuators_effective_0.control_effective[1]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl2     ",
-						   actuators_0.control_effective[2]);
+						   actuators_effective_0.control_effective[2]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl3     ",
-						   actuators_0.control_effective[3]);
+						   actuators_effective_0.control_effective[3]);
+	}
+}
+
+void
+l_vehicle_attitude_controls(const struct listener *l)
+{
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+
+	if (gcs_link) {
+		/* send, add spaces so that string buffer is at least 10 chars long */
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl0    ",
+						   actuators_0.control[0]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl1    ",
+						   actuators_0.control[1]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl2     ",
+						   actuators_0.control[2]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl3     ",
+						   actuators_0.control[3]);
 	}
 }
 
@@ -637,7 +666,7 @@ l_airspeed(const struct listener *l)
 	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
 
 	float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-	float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
+	float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
 	float alt = global_pos.alt;
 	float climb = global_pos.vz;
 
@@ -773,7 +802,10 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	orb_set_interval(mavlink_subs.actuators_effective_sub, 100);	/* 10Hz updates */
+
+	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 
 	/* --- DEBUG VALUE OUTPUT --- */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 506f73105..e2e630046 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -80,6 +80,7 @@ struct mavlink_subscriptions {
 	int safety_sub;
 	int actuators_sub;
 	int armed_sub;
+	int actuators_effective_sub;
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 3466841c4..760e598bc 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -64,9 +64,8 @@
 #include <mavlink/mavlink_log.h>
 #include <poll.h>
 #include <systemlib/err.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <systemlib/systemlib.h>
-#include <systemlib/conversions.h>
 #include <drivers/drv_hrt.h>
 
 #include "position_estimator_inav_params.h"
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index e01cc4dda..310fbf60f 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -42,7 +42,7 @@
 
 #include <stdio.h>
 #include <math.h>
-#include "conversions.h"
+#include <geo/geo.h>
 #include "airspeed.h"
 
 
@@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am
 float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
 {
 	float density = get_air_density(static_pressure, temperature_celsius);
+
 	if (density < 0.0001f || !isfinite(density)) {
-	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
-//	 printf("[airspeed] Invalid air density, using density at sea level\n");
+		density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
 	}
 
 	float pressure_difference = total_pressure - static_pressure;
 
-	if(pressure_difference > 0) {
+	if (pressure_difference > 0) {
 		return sqrtf((2.0f*(pressure_difference)) / density);
-	} else
-	{
+	} else {
 		return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
 	}
 }
+
+float get_air_density(float static_pressure, float temperature_celsius)
+{
+	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+}
diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h
index def53f0c1..8dccaab9c 100644
--- a/src/modules/systemlib/airspeed.h
+++ b/src/modules/systemlib/airspeed.h
@@ -85,6 +85,14 @@
   */
  __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
 
+ /**
+ * Calculates air density.
+ *
+ * @param static_pressure ambient pressure in millibar
+ * @param temperature_celcius air / ambient temperature in celcius
+ */
+__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
+
 __END_DECLS
 
 #endif
diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c
index ac94252c5..9105d83cb 100644
--- a/src/modules/systemlib/conversions.c
+++ b/src/modules/systemlib/conversions.c
@@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[])
 
 	return u.w;
 }
-
-void rot2quat(const float R[9], float Q[4])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-	int32_t idx;
-
-	/* conversion of rotation matrix to quaternion
-	 * choose the largest component to begin with */
-	q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F;
-	q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F;
-	q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F;
-	q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F;
-
-	idx = 0;
-
-	if (q0_2 < q1_2) {
-		q0_2 = q1_2;
-
-		idx = 1;
-	}
-
-	if (q0_2 < q2_2) {
-		q0_2 = q2_2;
-		idx = 2;
-	}
-
-	if (q0_2 < q3_2) {
-		q0_2 = q3_2;
-		idx = 3;
-	}
-
-	q0_2 = sqrtf(q0_2);
-
-	/* solve for the remaining three components */
-	if (idx == 0) {
-		q1_2 = q0_2;
-		q2_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q0_2 = (R[1] - R[3]) / 4.0F / q0_2;
-
-	} else if (idx == 1) {
-		q2_2 = q0_2;
-		q1_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[6] + R[2]) / 4.0F / q0_2;
-
-	} else if (idx == 2) {
-		q3_2 = q0_2;
-		q1_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q2_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[7] + R[5]) / 4.0F / q0_2;
-
-	} else {
-		q1_2 = (R[1] - R[3]) / 4.0F / q0_2;
-		q2_2 = (R[6] + R[2]) / 4.0F / q0_2;
-		q3_2 = (R[7] + R[5]) / 4.0F / q0_2;
-	}
-
-	/* return values */
-	Q[0] = q1_2;
-	Q[1] = q2_2;
-	Q[2] = q3_2;
-	Q[3] = q0_2;
-}
-
-void quat2rot(const float Q[4], float R[9])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-
-	memset(&R[0], 0, 9U * sizeof(float));
-
-	q0_2 = Q[0] * Q[0];
-	q1_2 = Q[1] * Q[1];
-	q2_2 = Q[2] * Q[2];
-	q3_2 = Q[3] * Q[3];
-
-	R[0] = ((q0_2 + q1_2) - q2_2) - q3_2;
-	R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]);
-	R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]);
-	R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]);
-	R[4] = ((q0_2 + q2_2) - q1_2) - q3_2;
-	R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]);
-	R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]);
-	R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]);
-	R[8] = ((q0_2 + q3_2) - q1_2) - q2_2;
-}
-
-float get_air_density(float static_pressure, float temperature_celsius)
-{
-	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
-}
diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 064426f21..dc383e770 100644
--- a/src/modules/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
@@ -43,7 +43,6 @@
 #define CONVERSIONS_H_
 #include <float.h>
 #include <stdint.h>
-#include <systemlib/geo/geo.h>
 
 __BEGIN_DECLS
 
@@ -57,34 +56,6 @@ __BEGIN_DECLS
  */
 __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]);
 
-/**
- * Converts a 3 x 3 rotation matrix to an unit quaternion.
- *
- * All orientations are expressed in NED frame.
- *
- * @param R rotation matrix to convert
- * @param Q quaternion to write back to
- */
-__EXPORT void rot2quat(const float R[9], float Q[4]);
-
-/**
- * Converts an unit quaternion to a 3 x 3 rotation matrix.
- *
- * All orientations are expressed in NED frame.
- *
- * @param Q quaternion to convert
- * @param R rotation matrix to write back to
- */
-__EXPORT void quat2rot(const float Q[4], float R[9]);
-
-/**
- * Calculates air density.
- *
- * @param static_pressure ambient pressure in millibar
- * @param temperature_celcius air / ambient temperature in celcius
- */
-__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
-
 __END_DECLS
 
 #endif /* CONVERSIONS_H_ */
diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c
deleted file mode 100644
index 6463e6489..000000000
--- a/src/modules/systemlib/geo/geo.c
+++ /dev/null
@@ -1,438 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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 geo.c
- *
- * Geo / math functions to perform geodesic calculations
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <systemlib/geo/geo.h>
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-
-
-/* values for map projection */
-static double phi_1;
-static double sin_phi_1;
-static double cos_phi_1;
-static double lambda_0;
-static double scale;
-
-__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
-{
-	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	phi_1 = lat_0 / 180.0 * M_PI;
-	lambda_0 = lon_0 / 180.0 * M_PI;
-
-	sin_phi_1 = sin(phi_1);
-	cos_phi_1 = cos(phi_1);
-
-	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
-
-	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-	const double r_earth = 6371000;
-
-	double lat1 = phi_1;
-	double lon1 = lambda_0;
-
-	double lat2 = phi_1 + 0.5 / 180 * M_PI;
-	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
-	double sin_lat_2 = sin(lat2);
-	double cos_lat_2 = cos(lat2);
-	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
-
-	/* 2) calculate distance rho on plane */
-	double k_bar = 0;
-	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
-	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
-	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
-
-	scale = d / rho;
-
-}
-
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
-{
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	double phi = lat / 180.0 * M_PI;
-	double lambda = lon / 180.0 * M_PI;
-
-	double sin_phi = sin(phi);
-	double cos_phi = cos(phi);
-
-	double k_bar = 0;
-	/* using small angle approximation (formula in comment is without aproximation) */
-	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	/* using small angle approximation (formula in comment is without aproximation) */
-	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
-	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
-
-//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
-}
-
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
-{
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-
-	double x_descaled = x / scale;
-	double y_descaled = y / scale;
-
-	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
-	double sin_c = sin(c);
-	double cos_c = cos(c);
-
-	double lat_sphere = 0;
-
-	if (c != 0)
-		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
-	else
-		lat_sphere = asin(cos_c * sin_phi_1);
-
-//	printf("lat_sphere = %.10f\n",lat_sphere);
-
-	double lon_sphere = 0;
-
-	if (phi_1  == M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
-
-	} else if (phi_1 == -M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
-
-	} else {
-
-		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
-		//using small angle approximation
-//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
-//    	if(denominator != 0)
-//    	{
-//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
-//    	}
-//    	else
-//    	{
-//    	...
-//    	}
-	}
-
-//	printf("lon_sphere = %.10f\n",lon_sphere);
-
-	*lat = lat_sphere * 180.0 / M_PI;
-	*lon = lon_sphere * 180.0 / M_PI;
-
-}
-
-
-__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
-{
-	double lat_now_rad = lat_now / 180.0d * M_PI;
-	double lon_now_rad = lon_now / 180.0d * M_PI;
-	double lat_next_rad = lat_next / 180.0d * M_PI;
-	double lon_next_rad = lon_next / 180.0d * M_PI;
-
-
-	double d_lat = lat_next_rad - lat_now_rad;
-	double d_lon = lon_next_rad - lon_now_rad;
-
-	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
-	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
-
-	const double radius_earth = 6371000.0d;
-	return radius_earth * c; 
-}
-
-__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
-{
-	double lat_now_rad = lat_now * M_DEG_TO_RAD;
-	double lon_now_rad = lon_now * M_DEG_TO_RAD;
-	double lat_next_rad = lat_next * M_DEG_TO_RAD;
-	double lon_next_rad = lon_next * M_DEG_TO_RAD;
-
-	double d_lat = lat_next_rad - lat_now_rad;
-	double d_lon = lon_next_rad - lon_now_rad;
-
-	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
-
-	theta = _wrap_pi(theta);
-
-	return theta;
-}
-
-// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
-
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
-{
-// This function returns the distance to the nearest point on the track line.  Distance is positive if current
-// position is right of the track and negative if left of the track as seen from a point on the track line
-// headed towards the end point.
-
-	float dist_to_end;
-	float bearing_end;
-	float bearing_track;
-	float bearing_diff;
-
-	int return_value = ERROR;	// Set error flag, cleared when valid result calculated.
-	crosstrack_error->past_end = false;
-	crosstrack_error->distance = 0.0f;
-	crosstrack_error->bearing = 0.0f;
-
-	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value;
-
-	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
-	bearing_diff = bearing_track - bearing_end;
-	bearing_diff = _wrap_pi(bearing_diff);
-
-	// Return past_end = true if past end point of line
-	if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
-		crosstrack_error->past_end = true;
-		return_value = OK;
-		return return_value;
-	}
-
-	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-	crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
-
-	if (sin(bearing_diff) >= 0) {
-		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
-
-	} else {
-		crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
-	}
-
-	return_value = OK;
-
-	return return_value;
-
-}
-
-
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
-		float radius, float arc_start_bearing, float arc_sweep)
-{
-	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
-	// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
-	// headed towards the end point.
-
-	// Determine if the current position is inside or outside the sector between the line from the center
-	// to the arc start and the line from the center to the arc end
-	float	bearing_sector_start;
-	float	bearing_sector_end;
-	float	bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
-	bool	in_sector;
-
-	int return_value = ERROR;		// Set error flag, cleared when valid result calculated.
-	crosstrack_error->past_end = false;
-	crosstrack_error->distance = 0.0f;
-	crosstrack_error->bearing = 0.0f;
-
-	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value;
-
-
-	if (arc_sweep >= 0) {
-		bearing_sector_start = arc_start_bearing;
-		bearing_sector_end = arc_start_bearing + arc_sweep;
-
-		if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
-
-	} else {
-		bearing_sector_end = arc_start_bearing;
-		bearing_sector_start = arc_start_bearing - arc_sweep;
-
-		if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F;
-	}
-
-	in_sector = false;
-
-	// Case where sector does not span zero
-	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
-
-	// Case where sector does span zero
-	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
-
-	// If in the sector then calculate distance and bearing to closest point
-	if (in_sector) {
-		crosstrack_error->past_end = false;
-		float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
-
-		if (dist_to_center <= radius) {
-			crosstrack_error->distance = radius - dist_to_center;
-			crosstrack_error->bearing = bearing_now + M_PI_F;
-
-		} else {
-			crosstrack_error->distance = dist_to_center - radius;
-			crosstrack_error->bearing = bearing_now;
-		}
-
-		// If out of the sector then calculate dist and bearing to start or end point
-
-	} else {
-
-		// Use the approximation  that 111,111 meters in the y direction is 1 degree (of latitude)
-		// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
-		// calculate the position of the start and end points.  We should not be doing this often
-		// as this function generally will not be called repeatedly when we are out of the sector.
-
-		// TO DO - this is messed up and won't compile
-		float start_disp_x = radius * sin(arc_start_bearing);
-		float start_disp_y = radius * cos(arc_start_bearing);
-		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
-		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
-		float lon_start = lon_now + start_disp_x / 111111.0d;
-		float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
-		float lon_end = lon_now + end_disp_x / 111111.0d;
-		float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
-		float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-		float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-
-
-		if (dist_to_start < dist_to_end) {
-			crosstrack_error->distance = dist_to_start;
-			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-
-		} else {
-			crosstrack_error->past_end = true;
-			crosstrack_error->distance = dist_to_end;
-			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-		}
-
-	}
-
-	crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
-	return_value = OK;
-	return return_value;
-}
-
-__EXPORT float _wrap_pi(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing) || bearing == 0) {
-		return bearing;
-	}
-
-	int c = 0;
-
-	while (bearing > M_PI_F && c < 30) {
-		bearing -= M_TWOPI_F;
-		c++;
-	}
-
-	c = 0;
-
-	while (bearing <=  -M_PI_F && c < 30) {
-		bearing += M_TWOPI_F;
-		c++;
-	}
-
-	return bearing;
-}
-
-__EXPORT float _wrap_2pi(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
-		return bearing;
-	}
-
-	while (bearing >= M_TWOPI_F) {
-		bearing = bearing - M_TWOPI_F;
-	}
-
-	while (bearing <  0.0f) {
-		bearing = bearing + M_TWOPI_F;
-	}
-
-	return bearing;
-}
-
-__EXPORT float _wrap_180(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
-		return bearing;
-	}
-
-	while (bearing > 180.0f) {
-		bearing = bearing - 360.0f;
-	}
-
-	while (bearing <=  -180.0f) {
-		bearing = bearing + 360.0f;
-	}
-
-	return bearing;
-}
-
-__EXPORT float _wrap_360(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
-		return bearing;
-	}
-
-	while (bearing >= 360.0f) {
-		bearing = bearing - 360.0f;
-	}
-
-	while (bearing <  0.0f) {
-		bearing = bearing + 360.0f;
-	}
-
-	return bearing;
-}
-
-
diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h
deleted file mode 100644
index dadec51ec..000000000
--- a/src/modules/systemlib/geo/geo.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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 geo.h
- *
- * Definition of geo / math functions to perform geodesic calculations
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
- */
-
-#pragma once
-
-__BEGIN_DECLS
-
-#include <stdbool.h>
-
-#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
-#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
-#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
-#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
-#define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
-
-/* compatibility aliases */
-#define RADIUS_OF_EARTH		CONSTANTS_RADIUS_OF_EARTH
-#define GRAVITY_MSS		CONSTANTS_ONE_G
-
-// XXX remove
-struct crosstrack_error_s {
-	bool past_end;		// Flag indicating we are past the end of the line/arc segment
-	float distance;		// Distance in meters to closest point on line/arc
-	float bearing;		// Bearing in radians to closest point on line/arc
-} ;
-
-/**
- * Initializes the map transformation.
- *
- * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT void map_projection_init(double lat_0, double lon_0);
-
-/**
- * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
-
-/**
- * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
- *
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
-
-/**
- * Returns the distance to the next waypoint in meters.
- *
- * @param lat_now current position in degrees (47.1234567°, not 471234567°)
- * @param lon_now current position in degrees (8.1234567°, not 81234567°)
- * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
- * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
-
-/**
- * Returns the bearing to the next waypoint in radians.
- *
- * @param lat_now current position in degrees (47.1234567°, not 471234567°)
- * @param lon_now current position in degrees (8.1234567°, not 81234567°)
- * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
- * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
-
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
-
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
-		float radius, float arc_start_bearing, float arc_sweep);
-
-__EXPORT float _wrap_180(float bearing);
-__EXPORT float _wrap_360(float bearing);
-__EXPORT float _wrap_pi(float bearing);
-__EXPORT float _wrap_2pi(float bearing);
-
-__END_DECLS
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index cbf829122..94c744c03 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -45,7 +45,6 @@ SRCS		 = err.c \
 		   getopt_long.c \
 		   up_cxxinitialize.c \
 		   pid/pid.c \
-		   geo/geo.c \
 		   systemlib.c \
 		   airspeed.c \
 		   system_params.c \
-- 
cgit v1.2.3