mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-01 08:00:35 +08:00
px4params don't search recursively for params
- move controllib_test under controllib
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
0dfd8cd039
commit
db816982cd
@@ -1,43 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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
|
||||
SRCS
|
||||
controllib_test_main.cpp
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
@@ -1,530 +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 blocks.cpp
|
||||
*
|
||||
* Controller library code
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <float.h>
|
||||
|
||||
#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();
|
||||
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;
|
||||
}
|
||||
|
||||
} // namespace control
|
||||
@@ -1,556 +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_test_main.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()
|
||||
{
|
||||
bool failed = false;
|
||||
failed = failed || blockLimitTest() < 0;
|
||||
failed = failed || blockLimitSymTest() < 0;
|
||||
failed = failed || blockLowPassTest() < 0;
|
||||
failed = failed || blockHighPassTest() < 0;
|
||||
failed = failed || blockLowPass2Test() < 0;
|
||||
failed = failed || blockIntegralTest() < 0;
|
||||
failed = failed || blockIntegralTrapTest() < 0;
|
||||
failed = failed || blockDerivativeTest() < 0;
|
||||
failed = failed || blockPTest() < 0;
|
||||
failed = failed || blockPITest() < 0;
|
||||
failed = failed || blockPDTest() < 0;
|
||||
failed = failed || blockPIDTest() < 0;
|
||||
failed = failed || blockOutputTest() < 0;
|
||||
// known failures
|
||||
//failed = failed || blockRandUniformTest() < 0;
|
||||
//failed = failed || blockRandGaussTest() < 0;
|
||||
failed = failed || blockStatsTest() < 0;
|
||||
failed = failed || blockDelayTest() < 0;
|
||||
return failed ? -1 : 0;
|
||||
}
|
||||
|
||||
int blockLimitTest()
|
||||
{
|
||||
printf("Test BlockLimit\t\t\t: ");
|
||||
BlockLimit limit(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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(nullptr, "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 = sqrtf(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(nullptr, "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(nullptr, "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;
|
||||
}
|
||||
@@ -1,66 +0,0 @@
|
||||
#include <systemlib/param/param.h>
|
||||
// WARNING:
|
||||
// do not changes these unless
|
||||
// you want to recompute the
|
||||
// answers for all of the unit tests
|
||||
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);
|
||||
@@ -39,7 +39,6 @@ px4_add_module(
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
position_estimator_inav_main.cpp
|
||||
position_estimator_inav_params.cpp
|
||||
inertial_filter.cpp
|
||||
DEPENDS
|
||||
platforms__common
|
||||
|
||||
-69
@@ -343,72 +343,3 @@ PARAM_DEFINE_FLOAT(INAV_LIDAR_OFF, 0.0f);
|
||||
* @group Position Estimator INAV
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
|
||||
|
||||
int inav_parameters_init(struct position_estimator_inav_param_handles *h)
|
||||
{
|
||||
h->w_z_baro = param_find("INAV_W_Z_BARO");
|
||||
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
|
||||
h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
|
||||
h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
|
||||
h->w_z_lidar = param_find("INAV_W_Z_LIDAR");
|
||||
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
|
||||
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
|
||||
h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
|
||||
h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
|
||||
h->w_mocap_p = param_find("INAV_W_MOC_P");
|
||||
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
|
||||
h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
|
||||
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
|
||||
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
|
||||
h->flow_k = param_find("INAV_FLOW_K");
|
||||
h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
|
||||
h->lidar_err = param_find("INAV_LIDAR_ERR");
|
||||
h->land_t = param_find("INAV_LAND_T");
|
||||
h->land_disp = param_find("INAV_LAND_DISP");
|
||||
h->land_thr = param_find("INAV_LAND_THR");
|
||||
h->no_vision = param_find("CBRK_NO_VISION");
|
||||
h->delay_gps = param_find("INAV_DELAY_GPS");
|
||||
h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
|
||||
h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
|
||||
h->disable_mocap = param_find("INAV_DISAB_MOCAP");
|
||||
h->enable_lidar_alt_est = param_find("INAV_LIDAR_EST");
|
||||
h->lidar_calibration_offset = param_find("INAV_LIDAR_OFF");
|
||||
h->att_ext_hdg_m = param_find("ATT_EXT_HDG_M");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int inav_parameters_update(const struct position_estimator_inav_param_handles *h,
|
||||
struct position_estimator_inav_params *p)
|
||||
{
|
||||
param_get(h->w_z_baro, &(p->w_z_baro));
|
||||
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
|
||||
param_get(h->w_z_gps_v, &(p->w_z_gps_v));
|
||||
param_get(h->w_z_vision_p, &(p->w_z_vision_p));
|
||||
param_get(h->w_z_lidar, &(p->w_z_lidar));
|
||||
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
|
||||
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
|
||||
param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
|
||||
param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
|
||||
param_get(h->w_mocap_p, &(p->w_mocap_p));
|
||||
param_get(h->w_xy_flow, &(p->w_xy_flow));
|
||||
param_get(h->w_xy_res_v, &(p->w_xy_res_v));
|
||||
param_get(h->w_gps_flow, &(p->w_gps_flow));
|
||||
param_get(h->w_acc_bias, &(p->w_acc_bias));
|
||||
param_get(h->flow_k, &(p->flow_k));
|
||||
param_get(h->flow_q_min, &(p->flow_q_min));
|
||||
param_get(h->lidar_err, &(p->lidar_err));
|
||||
param_get(h->land_t, &(p->land_t));
|
||||
param_get(h->land_disp, &(p->land_disp));
|
||||
param_get(h->land_thr, &(p->land_thr));
|
||||
param_get(h->no_vision, &(p->no_vision));
|
||||
param_get(h->delay_gps, &(p->delay_gps));
|
||||
param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
|
||||
param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
|
||||
param_get(h->disable_mocap, &(p->disable_mocap));
|
||||
param_get(h->enable_lidar_alt_est, &(p->enable_lidar_alt_est));
|
||||
param_get(h->lidar_calibration_offset, &(p->lidar_calibration_offset));
|
||||
param_get(h->att_ext_hdg_m, &(p->att_ext_hdg_m));
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1405,3 +1405,73 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
thread_running = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int inav_parameters_init(struct position_estimator_inav_param_handles *h)
|
||||
{
|
||||
h->w_z_baro = param_find("INAV_W_Z_BARO");
|
||||
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
|
||||
h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
|
||||
h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
|
||||
h->w_z_lidar = param_find("INAV_W_Z_LIDAR");
|
||||
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
|
||||
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
|
||||
h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
|
||||
h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
|
||||
h->w_mocap_p = param_find("INAV_W_MOC_P");
|
||||
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
|
||||
h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
|
||||
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
|
||||
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
|
||||
h->flow_k = param_find("INAV_FLOW_K");
|
||||
h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
|
||||
h->lidar_err = param_find("INAV_LIDAR_ERR");
|
||||
h->land_t = param_find("INAV_LAND_T");
|
||||
h->land_disp = param_find("INAV_LAND_DISP");
|
||||
h->land_thr = param_find("INAV_LAND_THR");
|
||||
h->no_vision = param_find("CBRK_NO_VISION");
|
||||
h->delay_gps = param_find("INAV_DELAY_GPS");
|
||||
h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
|
||||
h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
|
||||
h->disable_mocap = param_find("INAV_DISAB_MOCAP");
|
||||
h->enable_lidar_alt_est = param_find("INAV_LIDAR_EST");
|
||||
h->lidar_calibration_offset = param_find("INAV_LIDAR_OFF");
|
||||
h->att_ext_hdg_m = param_find("ATT_EXT_HDG_M");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int inav_parameters_update(const struct position_estimator_inav_param_handles *h,
|
||||
struct position_estimator_inav_params *p)
|
||||
{
|
||||
param_get(h->w_z_baro, &(p->w_z_baro));
|
||||
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
|
||||
param_get(h->w_z_gps_v, &(p->w_z_gps_v));
|
||||
param_get(h->w_z_vision_p, &(p->w_z_vision_p));
|
||||
param_get(h->w_z_lidar, &(p->w_z_lidar));
|
||||
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
|
||||
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
|
||||
param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
|
||||
param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
|
||||
param_get(h->w_mocap_p, &(p->w_mocap_p));
|
||||
param_get(h->w_xy_flow, &(p->w_xy_flow));
|
||||
param_get(h->w_xy_res_v, &(p->w_xy_res_v));
|
||||
param_get(h->w_gps_flow, &(p->w_gps_flow));
|
||||
param_get(h->w_acc_bias, &(p->w_acc_bias));
|
||||
param_get(h->flow_k, &(p->flow_k));
|
||||
param_get(h->flow_q_min, &(p->flow_q_min));
|
||||
param_get(h->lidar_err, &(p->lidar_err));
|
||||
param_get(h->land_t, &(p->land_t));
|
||||
param_get(h->land_disp, &(p->land_disp));
|
||||
param_get(h->land_thr, &(p->land_thr));
|
||||
param_get(h->no_vision, &(p->no_vision));
|
||||
param_get(h->delay_gps, &(p->delay_gps));
|
||||
param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
|
||||
param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
|
||||
param_get(h->disable_mocap, &(p->disable_mocap));
|
||||
param_get(h->enable_lidar_alt_est, &(p->enable_lidar_alt_est));
|
||||
param_get(h->lidar_calibration_offset, &(p->lidar_calibration_offset));
|
||||
param_get(h->att_ext_hdg_m, &(p->att_ext_hdg_m));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user