dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Moved control library.

This commit is contained in:
James Goppert 2016-04-14 14:07:28 -04:00
parent 831bce67bc
commit afcc5b4695
31 changed files with 787 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/check_code_style.sh
View File

@ -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 \

2
cmake/configs/nuttx_mindpx-v2_default.cmake
View File

@ -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

2
cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
View File

@ -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

2
cmake/configs/nuttx_px4fmu-v1_default.cmake
View File

@ -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

2
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@ -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

2
cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
View File

@ -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

2
cmake/configs/nuttx_px4fmu-v4_default.cmake
View File

@ -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

2
cmake/configs/posix_eagle_default.cmake
View File

@ -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

2
cmake/configs/posix_rpi2_default.cmake
View File

@ -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

2
cmake/configs/posix_rpi2_release.cmake
View File

@ -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

2
cmake/configs/posix_sitl_default.cmake
View File

@ -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

2
cmake/configs/posix_sitl_ekf2.cmake
View File

@ -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

2
cmake/configs/posix_sitl_replay.cmake
View File

@ -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

2
cmake/configs/qurt_eagle_default.cmake
View File

@ -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

2
cmake/configs/qurt_eagle_hil.cmake
View File

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

2
cmake/configs/qurt_eagle_release.cmake
View File

@ -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

2
cmake/configs/qurt_eagle_travis.cmake
View File

@ -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

7
src/modules/controllib/CMakeLists.txt → src/lib/controllib/CMakeLists.txt
View File

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

0
src/modules/controllib/block/Block.cpp → src/lib/controllib/block/Block.cpp
View File

0
src/modules/controllib/block/Block.hpp → src/lib/controllib/block/Block.hpp
View File

0
src/modules/controllib/block/BlockParam.cpp → src/lib/controllib/block/BlockParam.cpp
View File

0
src/modules/controllib/block/BlockParam.hpp → src/lib/controllib/block/BlockParam.hpp
View File

148
src/lib/controllib/blocks.cpp Normal file
View File

@ -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

36
src/modules/controllib/blocks.hpp → src/lib/controllib/blocks.hpp
View File

@ -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

0
src/modules/controllib/uorb/blocks.cpp → src/lib/controllib/uorb/blocks.cpp
View File

0
src/modules/controllib/uorb/blocks.hpp → src/lib/controllib/uorb/blocks.hpp
View File

51
src/modules/controllib/controllib_test_main.cpp
View File

@ -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;
}

45
src/modules/controllib_test/CMakeLists.txt Normal file
View File

@ -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 :

118
src/modules/controllib/blocks.cpp → src/modules/controllib_test/blocks.cpp
View File

@ -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: ");

555
src/modules/controllib_test/controllib_test_main.cpp Normal file
View File

@ -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;
}

0
src/modules/controllib/test_params.c → src/modules/controllib_test/test_params.c
View File