Merge pull request #4266 from jgoppert/controllib_move

Moved control library.

Tools/check_code_style.sh

@@ -29,7 +29,7 @@ for fn in $(find src/examples \
                  src/modules/local_position_estimator \
                  src/modules/unit_test \
                  src/modules/systemlib \
-                 src/modules/controllib \
+                 src/lib/controllib \
                    -path './Build' -prune -o \
                    -path './mavlink' -prune -o \
                    -path './NuttX' -prune -o \

cmake/configs/nuttx_mindpx-v2_default.cmake

@@ -112,13 +112,13 @@ set(config_module_list
 	modules/param
 	modules/systemlib
 	modules/systemlib/mixer
-	modules/controllib
 	modules/uORB
 	modules/dataman
 
 	#
 	# Libraries
 	#
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/ecl

cmake/configs/nuttx_px4-stm32f4discovery_default.cmake

@@ -30,13 +30,13 @@ set(config_module_list
 	modules/param
 	modules/systemlib
 	modules/systemlib/mixer
-	modules/controllib
 	modules/uORB
 
 	#
 	# Libraries
 	#
 	#lib/mathlib/CMSIS
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/ecl

cmake/configs/nuttx_px4fmu-v1_default.cmake

@@ -96,13 +96,13 @@ set(config_module_list
 	modules/param
 	modules/systemlib
 	modules/systemlib/mixer
-	modules/controllib
 	modules/uORB
 	modules/dataman
 
 	#
 	# Libraries
 	#
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/ecl

cmake/configs/nuttx_px4fmu-v2_default.cmake

@@ -106,13 +106,13 @@ set(config_module_list
 	modules/param
 	modules/systemlib
 	modules/systemlib/mixer
-	modules/controllib
 	modules/uORB
 	modules/dataman
 
 	#
 	# Libraries
 	#
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/ecl

cmake/configs/nuttx_px4fmu-v2_ekf2.cmake

@@ -105,13 +105,13 @@ set(config_module_list
 	modules/param
 	modules/systemlib
 	modules/systemlib/mixer
-	modules/controllib
 	modules/uORB
 	modules/dataman
 
 	#
 	# Libraries
 	#
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/ecl

cmake/configs/nuttx_px4fmu-v4_default.cmake

@@ -105,13 +105,13 @@ set(config_module_list
 	modules/param
 	modules/systemlib
 	modules/systemlib/mixer
-	modules/controllib
 	modules/uORB
 	modules/dataman
 
 	#
 	# Libraries
 	#
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/rc

cmake/configs/posix_eagle_default.cmake

@@ -43,8 +43,8 @@ set(config_module_list
 	modules/sdlog2
 	modules/simulator
 	modules/commander
-	modules/controllib
 
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/conversion

cmake/configs/posix_rpi2_default.cmake

@@ -32,7 +32,7 @@ set(config_module_list
 	modules/dataman
 	modules/sdlog2
 	modules/commander
-	modules/controllib
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/conversion

cmake/configs/posix_rpi2_release.cmake

@@ -41,7 +41,7 @@ set(config_module_list
 	modules/dataman
 	modules/sdlog2
 	modules/commander
-	modules/controllib
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/conversion

cmake/configs/posix_sitl_default.cmake

@@ -50,7 +50,7 @@ set(config_module_list
 	modules/dataman
 	modules/sdlog2
 	modules/commander
-	modules/controllib
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/conversion

cmake/configs/posix_sitl_ekf2.cmake

@@ -49,7 +49,7 @@ set(config_module_list
 	modules/dataman
 	modules/sdlog2
 	modules/commander
-	modules/controllib
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/conversion

cmake/configs/posix_sitl_replay.cmake

@@ -17,7 +17,7 @@ set(config_module_list
 	modules/ekf2
 	modules/ekf2_replay
 	modules/sdlog2
-	modules/controllib
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/conversion

cmake/configs/qurt_eagle_default.cmake

@@ -54,7 +54,6 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/commander
-	modules/controllib
 	modules/land_detector
 
 	#
@@ -67,6 +66,7 @@ set(config_module_list
 	#
 	# Libraries
 	#
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/geo

cmake/configs/qurt_eagle_hil.cmake

@@ -51,7 +51,7 @@ set(config_module_list
 	lib/terrain_estimation
 	lib/runway_takeoff
 	lib/tailsitter_recovery
-	modules/controllib
+	lib/controllib
 
 	#
 	# QuRT port

cmake/configs/qurt_eagle_release.cmake

@@ -69,11 +69,11 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/commander
-	modules/controllib
 
 	#
 	# Libraries
 	#
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/geo

cmake/configs/qurt_eagle_travis.cmake

@@ -52,11 +52,11 @@ set(config_module_list
 	modules/systemlib/mixer
 	modules/uORB
 	modules/commander
-	modules/controllib
 
 	#
 	# Libraries
 	#
+	lib/controllib
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/geo

src/lib/controllib/CMakeLists.txt

@@ -31,17 +31,14 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE modules__controllib
+	MODULE lib__controllib
-	MAIN controllib_test
 	COMPILE_FLAGS
 		-Os
 	SRCS
-		controllib_test_main.cpp
+		blocks.cpp
-		test_params.c
 		block/Block.cpp
 		block/BlockParam.cpp
 		uorb/blocks.cpp
-		blocks.cpp
 	DEPENDS
 		platforms__common
 	)

src/lib/controllib/blocks.cpp

@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file blocks.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <float.h>
+
+#include "blocks.hpp"
+
+#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; }
+
+namespace control
+{
+
+float BlockLimit::update(float input)
+{
+	if (input > getMax()) {
+		input = _max.get();
+
+	} else if (input < getMin()) {
+		input = getMin();
+	}
+
+	return input;
+}
+
+float BlockLimitSym::update(float input)
+{
+	if (input > getMax()) {
+		input = _max.get();
+
+	} else if (input < -getMax()) {
+		input = -getMax();
+	}
+
+	return input;
+}
+
+float BlockLowPass::update(float input)
+{
+	if (!PX4_ISFINITE(getState())) {
+		setState(input);
+	}
+
+	float b = 2 * float(M_PI) * getFCut() * getDt();
+	float a = b / (1 + b);
+	setState(a * input + (1 - a)*getState());
+	return getState();
+}
+
+float BlockHighPass::update(float input)
+{
+	float b = 2 * float(M_PI) * getFCut() * getDt();
+	float a = 1 / (1 + b);
+	setY(a * (getY() + input - getU()));
+	setU(input);
+	return getY();
+}
+
+float BlockLowPass2::update(float input)
+{
+	if (!PX4_ISFINITE(getState())) {
+		setState(input);
+	}
+
+	if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
+		_lp.set_cutoff_frequency(_fs, getFCutParam());
+	}
+
+	_state = _lp.apply(input);
+	return _state;
+}
+
+float BlockIntegral::update(float input)
+{
+	// trapezoidal integration
+	setY(_limit.update(getY() + input * getDt()));
+	return getY();
+}
+
+float BlockIntegralTrap::update(float input)
+{
+	// trapezoidal integration
+	setY(_limit.update(getY() +
+			   (getU() + input) / 2.0f * getDt()));
+	setU(input);
+	return getY();
+}
+
+float BlockDerivative::update(float input)
+{
+	float output;
+
+	if (_initialized) {
+		output = _lowPass.update((input - getU()) / getDt());
+
+	} else {
+		// if this is the first call to update
+		// we have no valid derivative
+		// and so we use the assumption the
+		// input value is not changing much,
+		// which is the best we can do here.
+		_lowPass.update(0.0f);
+		output = 0.0f;
+		_initialized = true;
+	}
+
+	setU(input);
+	return output;
+}
+
+} // namespace control

src/lib/controllib/blocks.hpp

@@ -55,8 +55,6 @@
 namespace control
 {
 
-int __EXPORT basicBlocksTest();
-
 /**
  * A limiter/ saturation.
  * The output of update is the input, bounded
@@ -82,8 +80,6 @@ protected:
 	control::BlockParamFloat _max;
 };
 
-int __EXPORT blockLimitTest();
-
 /**
  * A symmetric limiter/ saturation.
  * Same as limiter but with only a max, is used for
@@ -106,8 +102,6 @@ protected:
 	control::BlockParamFloat _max;
 };
 
-int __EXPORT blockLimitSymTest();
-
 /**
  * A low pass filter as described here:
  * http://en.wikipedia.org/wiki/Low-pass_filter.
@@ -133,8 +127,6 @@ protected:
 	control::BlockParamFloat _fCut;
 };
 
-int __EXPORT blockLowPassTest();
-
 /**
  * A high pass filter as described here:
  * http://en.wikipedia.org/wiki/High-pass_filter.
@@ -164,8 +156,6 @@ protected:
 	control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */
 };
 
-int __EXPORT blockHighPassTest();
-
 /**
  * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter
  */
@@ -194,8 +184,6 @@ protected:
 	math::LowPassFilter2p _lp;
 };
 
-int __EXPORT blockLowPass2Test();
-
 /**
  * A rectangular integrator.
  * A limiter is built into the class to bound the
@@ -223,8 +211,6 @@ protected:
 	BlockLimitSym _limit; /**< limiter */
 };
 
-int __EXPORT blockIntegralTest();
-
 /**
  * A trapezoidal integrator.
  * http://en.wikipedia.org/wiki/Trapezoidal_rule
@@ -257,8 +243,6 @@ protected:
 	BlockLimitSym _limit; /**< limiter */
 };
 
-int __EXPORT blockIntegralTrapTest();
-
 /**
  * A simple derivative approximation.
  * This uses the previous and current input.
@@ -304,8 +288,6 @@ protected:
 	BlockLowPass _lowPass; /**< low pass filter */
 };
 
-int __EXPORT blockDerivativeTest();
-
 /**
  * A proportional controller.
  * @link http://en.wikipedia.org/wiki/PID_controller
@@ -329,8 +311,6 @@ protected:
 	control::BlockParamFloat _kP;
 };
 
-int __EXPORT blockPTest();
-
 /**
  * A proportional-integral controller.
  * @link http://en.wikipedia.org/wiki/PID_controller
@@ -361,8 +341,6 @@ private:
 	control::BlockParamFloat _kI;
 };
 
-int __EXPORT blockPITest();
-
 /**
  * A proportional-derivative controller.
  * @link http://en.wikipedia.org/wiki/PID_controller
@@ -393,8 +371,6 @@ private:
 	control::BlockParamFloat _kD;
 };
 
-int __EXPORT blockPDTest();
-
 /**
  * A proportional-integral-derivative controller.
  * @link http://en.wikipedia.org/wiki/PID_controller
@@ -433,8 +409,6 @@ private:
 	control::BlockParamFloat _kD;
 };
 
-int __EXPORT blockPIDTest();
-
 /**
  * An output trim/ saturation block
  */
@@ -467,8 +441,6 @@ private:
 	float _val;
 };
 
-int __EXPORT blockOutputTest();
-
 /**
  * A uniform random number generator
  */
@@ -503,8 +475,6 @@ private:
 	control::BlockParamFloat _max;
 };
 
-int __EXPORT blockRandUniformTest();
-
 class __EXPORT BlockRandGauss: public Block
 {
 public:
@@ -553,8 +523,6 @@ private:
 	control::BlockParamFloat _stdDev;
 };
 
-int __EXPORT blockRandGaussTest();
-
 template<class Type, size_t M>
 class __EXPORT BlockStats: public Block
 {
@@ -599,8 +567,6 @@ private:
 	size_t _count;
 };
 
-int __EXPORT blockStatsTest();
-
 template<class Type, size_t M, size_t N, size_t LEN>
 class __EXPORT BlockDelay: public Block
 {
@@ -659,6 +625,4 @@ private:
 	int _delay;
 };
 
-int __EXPORT blockDelayTest();
-
 } // namespace control

src/modules/controllib/controllib_test_main.cpp

@@ -1,51 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2015 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 controllib.cpp
- * Unit testing for controllib.
- *
- * @author James Goppert <james.goppert@gmail.com>
- */
-
-#include "blocks.hpp"
-
-extern "C" __EXPORT int controllib_test_main(int argc, char *argv[]);
-
-int controllib_test_main(int argc, char *argv[])
-{
-	(void)argc;
-	(void)argv;
-	control::basicBlocksTest();
-	return 0;
-}

src/modules/controllib_test/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015 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_add_module(
+	MODULE modules__controllib_test
+	MAIN controllib_test
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		controllib_test_main.cpp
+		test_params.c
+	DEPENDS
+		platforms__common
+	)
+
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/controllib_test/blocks.cpp

@@ -41,13 +41,32 @@
 #include <stdio.h>
 #include <float.h>
 
-#include "blocks.hpp"
+#include <controllib/blocks.hpp>
 
 #define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; }
 
 namespace control
 {
 
+int basicBlocksTest();
+int blockLimitTest();
+int blockLimitSymTest();
+int blockLowPassTest();
+int blockHighPassTest();
+int blockLowPass2Test();
+int blockIntegralTest();
+int blockIntegralTrapTest();
+int blockDerivativeTest();
+int blockPTest();
+int blockPITest();
+int blockPDTest();
+int blockPIDTest();
+int blockOutputTest();
+int blockRandUniformTest();
+int blockRandGaussTest();
+int blockStatsTest();
+int blockDelayTest();
+
 int basicBlocksTest()
 {
 	blockLimitTest();
@@ -71,18 +90,6 @@ int basicBlocksTest()
 	return 0;
 }
 
-float BlockLimit::update(float input)
-{
-	if (input > getMax()) {
-		input = _max.get();
-
-	} else if (input < getMin()) {
-		input = getMin();
-	}
-
-	return input;
-}
-
 int blockLimitTest()
 {
 	printf("Test BlockLimit\t\t\t: ");
@@ -99,18 +106,6 @@ int blockLimitTest()
 	return 0;
 }
 
-float BlockLimitSym::update(float input)
-{
-	if (input > getMax()) {
-		input = _max.get();
-
-	} else if (input < -getMax()) {
-		input = -getMax();
-	}
-
-	return input;
-}
-
 int blockLimitSymTest()
 {
 	printf("Test BlockLimitSym\t\t: ");
@@ -126,18 +121,6 @@ int blockLimitSymTest()
 	return 0;
 }
 
-float BlockLowPass::update(float input)
-{
-	if (!PX4_ISFINITE(getState())) {
-		setState(input);
-	}
-
-	float b = 2 * float(M_PI) * getFCut() * getDt();
-	float a = b / (1 + b);
-	setState(a * input + (1 - a)*getState());
-	return getState();
-}
-
 int blockLowPassTest()
 {
 	printf("Test BlockLowPass\t\t: ");
@@ -166,15 +149,6 @@ int blockLowPassTest()
 	return 0;
 };
 
-float BlockHighPass::update(float input)
-{
-	float b = 2 * float(M_PI) * getFCut() * getDt();
-	float a = 1 / (1 + b);
-	setY(a * (getY() + input - getU()));
-	setU(input);
-	return getY();
-}
-
 int blockHighPassTest()
 {
 	printf("Test BlockHighPass\t\t: ");
@@ -206,20 +180,6 @@ int blockHighPassTest()
 	return 0;
 }
 
-float BlockLowPass2::update(float input)
-{
-	if (!PX4_ISFINITE(getState())) {
-		setState(input);
-	}
-
-	if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
-		_lp.set_cutoff_frequency(_fs, getFCutParam());
-	}
-
-	_state = _lp.apply(input);
-	return _state;
-}
-
 int blockLowPass2Test()
 {
 	printf("Test BlockLowPass2\t\t: ");
@@ -248,13 +208,6 @@ int blockLowPass2Test()
 	return 0;
 };
 
-float BlockIntegral::update(float input)
-{
-	// trapezoidal integration
-	setY(_limit.update(getY() + input * getDt()));
-	return getY();
-}
-
 int blockIntegralTest()
 {
 	printf("Test BlockIntegral\t\t: ");
@@ -292,15 +245,6 @@ int blockIntegralTest()
 	return 0;
 }
 
-float BlockIntegralTrap::update(float input)
-{
-	// trapezoidal integration
-	setY(_limit.update(getY() +
-			   (getU() + input) / 2.0f * getDt()));
-	setU(input);
-	return getY();
-}
-
 int blockIntegralTrapTest()
 {
 	printf("Test BlockIntegralTrap\t\t: ");
@@ -344,28 +288,6 @@ int blockIntegralTrapTest()
 	return 0;
 }
 
-float BlockDerivative::update(float input)
-{
-	float output;
-
-	if (_initialized) {
-		output = _lowPass.update((input - getU()) / getDt());
-
-	} else {
-		// if this is the first call to update
-		// we have no valid derivative
-		// and so we use the assumption the
-		// input value is not changing much,
-		// which is the best we can do here.
-		_lowPass.update(0.0f);
-		output = 0.0f;
-		_initialized = true;
-	}
-
-	setU(input);
-	return output;
-}
-
 int blockDerivativeTest()
 {
 	printf("Test BlockDerivative\t\t: ");

src/modules/controllib_test/controllib_test_main.cpp

@@ -0,0 +1,555 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 controllib.cpp
+ * Unit testing for controllib.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <float.h>
+
+#include <controllib/blocks.hpp>
+
+using namespace control;
+
+#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; }
+
+int basicBlocksTest();
+int blockLimitTest();
+int blockLimitSymTest();
+int blockLowPassTest();
+int blockHighPassTest();
+int blockLowPass2Test();
+int blockIntegralTest();
+int blockIntegralTrapTest();
+int blockDerivativeTest();
+int blockPTest();
+int blockPITest();
+int blockPDTest();
+int blockPIDTest();
+int blockOutputTest();
+int blockRandUniformTest();
+int blockRandGaussTest();
+int blockStatsTest();
+int blockDelayTest();
+
+int basicBlocksTest()
+{
+	blockLimitTest();
+	blockLimitSymTest();
+	blockLowPassTest();
+	blockHighPassTest();
+	blockLowPass2Test();
+	blockIntegralTest();
+	blockIntegralTrapTest();
+	blockDerivativeTest();
+	blockPTest();
+	blockPITest();
+	blockPDTest();
+	blockPIDTest();
+	blockOutputTest();
+	//blockRandUniformTest();
+	// known failures
+	// blockRandGaussTest();
+	blockStatsTest();
+	blockDelayTest();
+	return 0;
+}
+
+int blockLimitTest()
+{
+	printf("Test BlockLimit\t\t\t: ");
+	BlockLimit limit(NULL, "TEST");
+	// initial state
+	ASSERT_CL(equal(1.0f, limit.getMax()));
+	ASSERT_CL(equal(-1.0f, limit.getMin()));
+	ASSERT_CL(equal(0.0f, limit.getDt()));
+	// update
+	ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT_CL(equal(1.0f, limit.update(2.0f)));
+	ASSERT_CL(equal(0.0f, limit.update(0.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockLimitSymTest()
+{
+	printf("Test BlockLimitSym\t\t: ");
+	BlockLimitSym limit(NULL, "TEST");
+	// initial state
+	ASSERT_CL(equal(1.0f, limit.getMax()));
+	ASSERT_CL(equal(0.0f, limit.getDt()));
+	// update
+	ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT_CL(equal(1.0f, limit.update(2.0f)));
+	ASSERT_CL(equal(0.0f, limit.update(0.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockLowPassTest()
+{
+	printf("Test BlockLowPass\t\t: ");
+	BlockLowPass lowPass(NULL, "TEST_LP");
+	// test initial state
+	ASSERT_CL(equal(10.0f, lowPass.getFCut()));
+	ASSERT_CL(equal(0.0f, lowPass.getState()));
+	ASSERT_CL(equal(0.0f, lowPass.getDt()));
+	// set dt
+	lowPass.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, lowPass.getDt()));
+	// set state
+	lowPass.setState(1.0f);
+	ASSERT_CL(equal(1.0f, lowPass.getState()));
+	// test update
+	ASSERT_CL(equal(1.8626974f, lowPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		lowPass.update(2.0f);
+	}
+
+	ASSERT_CL(equal(2.0f, lowPass.getState()));
+	ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+};
+
+int blockHighPassTest()
+{
+	printf("Test BlockHighPass\t\t: ");
+	BlockHighPass highPass(NULL, "TEST_HP");
+	// test initial state
+	ASSERT_CL(equal(10.0f, highPass.getFCut()));
+	ASSERT_CL(equal(0.0f, highPass.getU()));
+	ASSERT_CL(equal(0.0f, highPass.getY()));
+	ASSERT_CL(equal(0.0f, highPass.getDt()));
+	// set dt
+	highPass.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, highPass.getDt()));
+	// set state
+	highPass.setU(1.0f);
+	ASSERT_CL(equal(1.0f, highPass.getU()));
+	highPass.setY(1.0f);
+	ASSERT_CL(equal(1.0f, highPass.getY()));
+	// test update
+	ASSERT_CL(equal(0.2746051f, highPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		highPass.update(2.0f);
+	}
+
+	ASSERT_CL(equal(0.0f, highPass.getY()));
+	ASSERT_CL(equal(0.0f, highPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockLowPass2Test()
+{
+	printf("Test BlockLowPass2\t\t: ");
+	BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
+	// test initial state
+	ASSERT_CL(equal(10.0f, lowPass.getFCutParam()));
+	ASSERT_CL(equal(0.0f, lowPass.getState()));
+	ASSERT_CL(equal(0.0f, lowPass.getDt()));
+	// set dt
+	lowPass.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, lowPass.getDt()));
+	// set state
+	lowPass.setState(1.0f);
+	ASSERT_CL(equal(1.0f, lowPass.getState()));
+	// test update
+	ASSERT_CL(equal(1.06745527f, lowPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		lowPass.update(2.0f);
+	}
+
+	ASSERT_CL(equal(2.0f, lowPass.getState()));
+	ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+};
+
+int blockIntegralTest()
+{
+	printf("Test BlockIntegral\t\t: ");
+	BlockIntegral integral(NULL, "TEST_I");
+	// test initial state
+	ASSERT_CL(equal(1.0f, integral.getMax()));
+	ASSERT_CL(equal(0.0f, integral.getDt()));
+	// set dt
+	integral.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, integral.getDt()));
+	// set Y
+	integral.setY(0.9f);
+	ASSERT_CL(equal(0.9f, integral.getY()));
+
+	// test exceed max
+	for (int i = 0; i < 100; i++) {
+		integral.update(1.0f);
+	}
+
+	ASSERT_CL(equal(1.0f, integral.update(1.0f)));
+	// test exceed min
+	integral.setY(-0.9f);
+	ASSERT_CL(equal(-0.9f, integral.getY()));
+
+	for (int i = 0; i < 100; i++) {
+		integral.update(-1.0f);
+	}
+
+	ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
+	// test update
+	integral.setY(0.1f);
+	ASSERT_CL(equal(0.2f, integral.update(1.0)));
+	ASSERT_CL(equal(0.2f, integral.getY()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockIntegralTrapTest()
+{
+	printf("Test BlockIntegralTrap\t\t: ");
+	BlockIntegralTrap integral(NULL, "TEST_I");
+	// test initial state
+	ASSERT_CL(equal(1.0f, integral.getMax()));
+	ASSERT_CL(equal(0.0f, integral.getDt()));
+	// set dt
+	integral.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, integral.getDt()));
+	// set U
+	integral.setU(1.0f);
+	ASSERT_CL(equal(1.0f, integral.getU()));
+	// set Y
+	integral.setY(0.9f);
+	ASSERT_CL(equal(0.9f, integral.getY()));
+
+	// test exceed max
+	for (int i = 0; i < 100; i++) {
+		integral.update(1.0f);
+	}
+
+	ASSERT_CL(equal(1.0f, integral.update(1.0f)));
+	// test exceed min
+	integral.setU(-1.0f);
+	integral.setY(-0.9f);
+	ASSERT_CL(equal(-0.9f, integral.getY()));
+
+	for (int i = 0; i < 100; i++) {
+		integral.update(-1.0f);
+	}
+
+	ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
+	// test update
+	integral.setU(2.0f);
+	integral.setY(0.1f);
+	ASSERT_CL(equal(0.25f, integral.update(1.0)));
+	ASSERT_CL(equal(0.25f, integral.getY()));
+	ASSERT_CL(equal(1.0f, integral.getU()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockDerivativeTest()
+{
+	printf("Test BlockDerivative\t\t: ");
+	BlockDerivative derivative(NULL, "TEST_D");
+	// test initial state
+	ASSERT_CL(equal(0.0f, derivative.getU()));
+	ASSERT_CL(equal(10.0f, derivative.getLP()));
+	// set dt
+	derivative.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, derivative.getDt()));
+	// set U
+	derivative.setU(1.0f);
+	ASSERT_CL(equal(1.0f, derivative.getU()));
+	// perform one update so initialized is set
+	derivative.update(1.0);
+	ASSERT_CL(equal(1.0f, derivative.getU()));
+	// test  update
+	ASSERT_CL(equal(8.6269744f, derivative.update(2.0f)));
+	ASSERT_CL(equal(2.0f, derivative.getU()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPTest()
+{
+	printf("Test BlockP\t\t\t: ");
+	BlockP blockP(NULL, "TEST_P");
+	// test initial state
+	ASSERT_CL(equal(0.2f, blockP.getKP()));
+	ASSERT_CL(equal(0.0f, blockP.getDt()));
+	// set dt
+	blockP.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, blockP.getDt()));
+	// test  update
+	ASSERT_CL(equal(0.4f, blockP.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPITest()
+{
+	printf("Test BlockPI\t\t\t: ");
+	BlockPI blockPI(NULL, "TEST");
+	// test initial state
+	ASSERT_CL(equal(0.2f, blockPI.getKP()));
+	ASSERT_CL(equal(0.1f, blockPI.getKI()));
+	ASSERT_CL(equal(0.0f, blockPI.getDt()));
+	ASSERT_CL(equal(1.0f, blockPI.getIntegral().getMax()));
+	// set dt
+	blockPI.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, blockPI.getDt()));
+	// set integral state
+	blockPI.getIntegral().setY(0.1f);
+	ASSERT_CL(equal(0.1f, blockPI.getIntegral().getY()));
+	// test  update
+	// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
+	ASSERT_CL(equal(0.43f, blockPI.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPDTest()
+{
+	printf("Test BlockPD\t\t\t: ");
+	BlockPD blockPD(NULL, "TEST");
+	// test initial state
+	ASSERT_CL(equal(0.2f, blockPD.getKP()));
+	ASSERT_CL(equal(0.01f, blockPD.getKD()));
+	ASSERT_CL(equal(0.0f, blockPD.getDt()));
+	ASSERT_CL(equal(10.0f, blockPD.getDerivative().getLP()));
+	// set dt
+	blockPD.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, blockPD.getDt()));
+	// set derivative state
+	blockPD.getDerivative().setU(1.0f);
+	ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU()));
+	// perform one update so initialized is set
+	blockPD.getDerivative().update(1.0);
+	ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU()));
+	// test  update
+	// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
+	ASSERT_CL(equal(0.486269744f, blockPD.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPIDTest()
+{
+	printf("Test BlockPID\t\t\t: ");
+	BlockPID blockPID(NULL, "TEST");
+	// test initial state
+	ASSERT_CL(equal(0.2f, blockPID.getKP()));
+	ASSERT_CL(equal(0.1f, blockPID.getKI()));
+	ASSERT_CL(equal(0.01f, blockPID.getKD()));
+	ASSERT_CL(equal(0.0f, blockPID.getDt()));
+	ASSERT_CL(equal(10.0f, blockPID.getDerivative().getLP()));
+	ASSERT_CL(equal(1.0f, blockPID.getIntegral().getMax()));
+	// set dt
+	blockPID.setDt(0.1f);
+	ASSERT_CL(equal(0.1f, blockPID.getDt()));
+	// set derivative state
+	blockPID.getDerivative().setU(1.0f);
+	ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU()));
+	// perform one update so initialized is set
+	blockPID.getDerivative().update(1.0);
+	ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU()));
+	// set integral state
+	blockPID.getIntegral().setY(0.1f);
+	ASSERT_CL(equal(0.1f, blockPID.getIntegral().getY()));
+	// test  update
+	// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
+	ASSERT_CL(equal(0.5162697f, blockPID.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockOutputTest()
+{
+	printf("Test BlockOutput\t\t: ");
+	BlockOutput blockOutput(NULL, "TEST");
+	// test initial state
+	ASSERT_CL(equal(0.0f, blockOutput.getDt()));
+	ASSERT_CL(equal(0.5f, blockOutput.get()));
+	ASSERT_CL(equal(-1.0f, blockOutput.getMin()));
+	ASSERT_CL(equal(1.0f, blockOutput.getMax()));
+	// test update below min
+	blockOutput.update(-2.0f);
+	ASSERT_CL(equal(-1.0f, blockOutput.get()));
+	// test update above max
+	blockOutput.update(2.0f);
+	ASSERT_CL(equal(1.0f, blockOutput.get()));
+	// test trim
+	blockOutput.update(0.0f);
+	ASSERT_CL(equal(0.5f, blockOutput.get()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockRandUniformTest()
+{
+	srand(1234);
+	printf("Test BlockRandUniform\t\t: ");
+	BlockRandUniform blockRandUniform(NULL, "TEST");
+	// test initial state
+	ASSERT_CL(equal(0.0f, blockRandUniform.getDt()));
+	ASSERT_CL(equal(-1.0f, blockRandUniform.getMin()));
+	ASSERT_CL(equal(1.0f, blockRandUniform.getMax()));
+	// test update
+	int n = 10000;
+	float mean = blockRandUniform.update();
+
+	for (int i = 2; i < n + 1; i++) {
+		float val = blockRandUniform.update();
+		mean += (val - mean) / i;
+		ASSERT_CL(less_than_or_equal(val, blockRandUniform.getMax()));
+		ASSERT_CL(greater_than_or_equal(val, blockRandUniform.getMin()));
+	}
+
+	ASSERT_CL(equal(mean, (blockRandUniform.getMin() +
+			       blockRandUniform.getMax()) / 2, 1e-1));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockRandGaussTest()
+{
+	srand(1234);
+	printf("Test BlockRandGauss\t\t: ");
+	BlockRandGauss blockRandGauss(NULL, "TEST");
+	// test initial state
+	ASSERT_CL(equal(0.0f, blockRandGauss.getDt()));
+	ASSERT_CL(equal(1.0f, blockRandGauss.getMean()));
+	ASSERT_CL(equal(2.0f, blockRandGauss.getStdDev()));
+	// test update
+	int n = 10000;
+	float mean = blockRandGauss.update();
+	float sum = 0;
+
+	// recursive mean, stdev algorithm from Knuth
+	for (int i = 2; i < n + 1; i++) {
+		float val = blockRandGauss.update();
+		float newMean = mean + (val - mean) / i;
+		sum += (val - mean) * (val - newMean);
+		mean = newMean;
+	}
+
+	float stdDev = sqrt(sum / (n - 1));
+	(void)(stdDev);
+	ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1));
+	ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockStatsTest()
+{
+	printf("Test BlockStats\t\t\t: ");
+	BlockStats<float, 1> stats(NULL, "TEST");
+	ASSERT_CL(equal(0.0f, stats.getMean()(0)));
+	ASSERT_CL(equal(0.0f, stats.getStdDev()(0)));
+	stats.update(matrix::Scalar<float>(1.0f));
+	stats.update(matrix::Scalar<float>(2));
+	ASSERT_CL(equal(1.5f, stats.getMean()(0)));
+	ASSERT_CL(equal(0.5f, stats.getStdDev()(0)));
+	stats.reset();
+	ASSERT_CL(equal(0.0f, stats.getMean()(0)));
+	ASSERT_CL(equal(0.0f, stats.getStdDev()(0)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockDelayTest()
+{
+	printf("Test BlockDelay\t\t\t: ");
+	using namespace matrix;
+	BlockDelay<float, 2, 1, 3> delay(NULL, "TEST");
+	Vector2f u1(1, 2);
+	Vector2f y1 = delay.update(u1);
+	ASSERT_CL(equal(y1(0), u1(0)));
+	ASSERT_CL(equal(y1(1), u1(1)));
+
+	Vector2f u2(4, 5);
+	Vector2f y2 = delay.update(u2);
+	ASSERT_CL(equal(y2(0), u1(0)));
+	ASSERT_CL(equal(y2(1), u1(1)));
+
+	Vector2f u3(7, 8);
+	Vector2f y3 = delay.update(u3);
+	ASSERT_CL(equal(y3(0), u1(0)));
+	ASSERT_CL(equal(y3(1), u1(1)));
+
+	Vector2f u4(9, 10);
+	Vector2f y4 = delay.update(u4);
+	ASSERT_CL(equal(y4(0), u2(0)));
+	ASSERT_CL(equal(y4(1), u2(1)));
+	printf("PASS\n");
+	return 0;
+}
+
+extern "C" __EXPORT int controllib_test_main(int argc, char *argv[]);
+
+int controllib_test_main(int argc, char *argv[])
+{
+	(void)argc;
+	(void)argv;
+	blockLimitTest();
+	blockLimitSymTest();
+	blockLowPassTest();
+	blockHighPassTest();
+	blockLowPass2Test();
+	blockIntegralTest();
+	blockIntegralTrapTest();
+	blockDerivativeTest();
+	blockPTest();
+	blockPITest();
+	blockPDTest();
+	blockPIDTest();
+	blockOutputTest();
+	//blockRandUniformTest();
+	// known failures
+	// blockRandGaussTest();
+	blockStatsTest();
+	blockDelayTest();
+	return 0;
+}