Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge pull request #4266 from jgoppert/controllib_move

Browse Source 
Moved control library.
This commit is contained in:
James Goppert
2016-04-14 14:29:16 -04:00
parent 831bce67bc afcc5b4695
commit ba36183d7d
31 changed files with 787 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/check_code_style.sh
+1 -1
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 \
cmake/configs/nuttx_mindpx-v2_default.cmake
+1 -1
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
cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
+1 -1
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
cmake/configs/nuttx_px4fmu-v1_default.cmake
+1 -1
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
cmake/configs/nuttx_px4fmu-v2_default.cmake
+1 -1
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
cmake/configs/nuttx_px4fmu-v2_ekf2.cmake
+1 -1
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
cmake/configs/nuttx_px4fmu-v4_default.cmake
+1 -1
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
cmake/configs/posix_eagle_default.cmake
+1 -1
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
cmake/configs/posix_rpi2_default.cmake
+1 -1
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
cmake/configs/posix_rpi2_release.cmake
+1 -1
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
cmake/configs/posix_sitl_default.cmake
+1 -1
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
cmake/configs/posix_sitl_ekf2.cmake
+1 -1
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
cmake/configs/posix_sitl_replay.cmake
+1 -1
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
cmake/configs/qurt_eagle_default.cmake
+1 -1
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
cmake/configs/qurt_eagle_hil.cmake
+1 -1
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
cmake/configs/qurt_eagle_release.cmake
+1 -1
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
cmake/configs/qurt_eagle_travis.cmake
+1 -1
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
src/modules/controllib/CMakeLists.txt → src/lib/controllib/CMakeLists.txt
+2 -5
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
)
src/modules/controllib/block/Block.cpp → src/lib/controllib/block/Block.cpp
View File
src/modules/controllib/block/Block.hpp → src/lib/controllib/block/Block.hpp
View File
src/modules/controllib/block/BlockParam.cpp → src/lib/controllib/block/BlockParam.cpp
View File
src/modules/controllib/block/BlockParam.hpp → src/lib/controllib/block/BlockParam.hpp
View File
src/lib/controllib/blocks.cpp
+148
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
src/modules/controllib/blocks.hpp → src/lib/controllib/blocks.hpp
-36
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
src/modules/controllib/uorb/blocks.cpp → src/lib/controllib/uorb/blocks.cpp
View File
src/modules/controllib/uorb/blocks.hpp → src/lib/controllib/uorb/blocks.hpp
View File
src/modules/controllib/controllib_test_main.cpp
-51
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;
}
src/modules/controllib_test/CMakeLists.txt
+45
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 :
src/modules/controllib/blocks.cpp → src/modules/controllib_test/blocks.cpp
+20 -98
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: ");
src/modules/controllib_test/controllib_test_main.cpp
+555
View File
File diff suppressed because it is too large Load Diff
src/modules/controllib/test_params.c → src/modules/controllib_test/test_params.c
View File