fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 04:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Moved control library.

Browse Source
This commit is contained in:
James Goppert
2016-04-14 14:28:32 -04:00
parent 831bce67bc
commit afcc5b4695
31 changed files with 787 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/controllib/block/Block.cpp
-216
View File
@@ -1,216 +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 Block.cpp
*
* Controller library code
*/
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include "Block.hpp"
#include "BlockParam.hpp"
namespace control
{
Block::Block(SuperBlock *parent, const char *name) :
_name(name),
_parent(parent),
_dt(0),
_subscriptions(),
_params()
{
if (getParent() != NULL) {
getParent()->getChildren().add(this);
}
}
void Block::getName(char *buf, size_t n)
{
if (getParent() == NULL) {
strncpy(buf, _name, n);
} else {
char parentName[blockNameLengthMax];
getParent()->getName(parentName, n);
if (!strcmp(_name, "")) {
strncpy(buf, parentName, blockNameLengthMax);
} else {
snprintf(buf, blockNameLengthMax, "%s_%s", parentName, _name);
}
}
}
void Block::updateParams()
{
BlockParamBase *param = getParams().getHead();
int count = 0;
while (param != NULL) {
if (count++ > maxParamsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max params for block: %s\n", name);
break;
}
//printf("updating param: %s\n", param->getName());
param->update();
param = param->getSibling();
}
}
void Block::updateSubscriptions()
{
uORB::SubscriptionNode *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
if (count++ > maxSubscriptionsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max subscriptions for block: %s\n", name);
break;
}
sub->update();
sub = sub->getSibling();
}
}
void Block::updatePublications()
{
uORB::PublicationNode *pub = getPublications().getHead();
int count = 0;
while (pub != NULL) {
if (count++ > maxPublicationsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max publications for block: %s\n", name);
break;
}
pub->update();
pub = pub->getSibling();
}
}
void SuperBlock::setDt(float dt)
{
Block::setDt(dt);
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->setDt(dt);
child = child->getSibling();
}
}
void SuperBlock::updateChildParams()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->updateParams();
child = child->getSibling();
}
}
void SuperBlock::updateChildSubscriptions()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->updateSubscriptions();
child = child->getSibling();
}
}
void SuperBlock::updateChildPublications()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->updatePublications();
child = child->getSibling();
}
}
} // namespace control
template class List<uORB::SubscriptionNode *>;
template class List<uORB::PublicationNode *>;
template class List<control::BlockParamBase *>;
src/modules/controllib/block/Block.hpp
-143
View File
@@ -1,143 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Block.h
*
* Controller library code
*/
#pragma once
#define __STDC_FORMAT_MACROS
#include <stdint.h>
#include <inttypes.h>
#include <containers/List.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <controllib/block/BlockParam.hpp>
namespace control
{
static const uint16_t maxChildrenPerBlock = 100;
static const uint16_t maxParamsPerBlock = 100;
static const uint16_t maxSubscriptionsPerBlock = 100;
static const uint16_t maxPublicationsPerBlock = 100;
static const uint8_t blockNameLengthMax = 80;
// forward declaration
class BlockParamBase;
class SuperBlock;
/**
*/
class __EXPORT Block :
public ListNode<Block *>
{
public:
friend class BlockParamBase;
// methods
Block(SuperBlock *parent, const char *name);
void getName(char *name, size_t n);
virtual ~Block() {};
virtual void updateParams();
virtual void updateSubscriptions();
virtual void updatePublications();
virtual void setDt(float dt) { _dt = dt; }
// accessors
float getDt() { return _dt; }
protected:
// accessors
SuperBlock *getParent() { return _parent; }
List<uORB::SubscriptionNode *> &getSubscriptions() { return _subscriptions; }
List<uORB::PublicationNode *> &getPublications() { return _publications; }
List<BlockParamBase *> &getParams() { return _params; }
// attributes
const char *_name;
SuperBlock *_parent;
float _dt;
List<uORB::SubscriptionNode *> _subscriptions;
List<uORB::PublicationNode *> _publications;
List<BlockParamBase *> _params;
private:
/* this class has pointer data members and should not be copied (private constructor) */
Block(const control::Block &);
Block operator=(const control::Block &);
};
class __EXPORT SuperBlock :
public Block
{
public:
friend class Block;
// methods
SuperBlock(SuperBlock *parent, const char *name) :
Block(parent, name),
_children()
{
}
virtual ~SuperBlock() {};
virtual void setDt(float dt);
virtual void updateParams()
{
Block::updateParams();
if (getChildren().getHead() != NULL) { updateChildParams(); }
}
virtual void updateSubscriptions()
{
Block::updateSubscriptions();
if (getChildren().getHead() != NULL) { updateChildSubscriptions(); }
}
virtual void updatePublications()
{
Block::updatePublications();
if (getChildren().getHead() != NULL) { updateChildPublications(); }
}
protected:
// methods
List<Block *> &getChildren() { return _children; }
void updateChildParams();
void updateChildSubscriptions();
void updateChildPublications();
// attributes
List<Block *> _children;
};
} // namespace control
src/modules/controllib/block/BlockParam.cpp
-130
View File
@@ -1,130 +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 Blockparam.cpp
*
* Controller library code
*/
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "BlockParam.hpp"
#include <containers/List.hpp>
namespace control
{
BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_prefix) :
_handle(PARAM_INVALID)
{
char fullname[blockNameLengthMax];
if (parent == NULL) {
strncpy(fullname, name, blockNameLengthMax);
} else {
char parentName[blockNameLengthMax];
parent->getName(parentName, blockNameLengthMax);
if (!strcmp(name, "")) {
strncpy(fullname, parentName, blockNameLengthMax);
} else if (parent_prefix) {
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
} else {
strncpy(fullname, name, blockNameLengthMax);
}
parent->getParams().add(this);
}
_handle = param_find(fullname);
if (_handle == PARAM_INVALID) {
printf("error finding param: %s\n", fullname);
}
};
template <class T>
BlockParam<T>::BlockParam(Block *block, const char *name,
bool parent_prefix, T *extern_address) :
BlockParamBase(block, name, parent_prefix),
_val(),
_extern_address(extern_address)
{
update();
}
template <class T>
T BlockParam<T>::get() { return _val; }
template <class T>
void BlockParam<T>::set(T val)
{
_val = val;
if (_extern_address != NULL) {
*_extern_address = val;
}
}
template <class T>
void BlockParam<T>::update()
{
if (_handle != PARAM_INVALID) {
param_get(_handle, &_val);
if (_extern_address != NULL) {
*_extern_address = _val;
}
}
}
template <class T>
void BlockParam<T>::commit()
{
if (_handle != PARAM_INVALID) { param_set(_handle, &_val); }
}
template <class T>
BlockParam<T>::~BlockParam() {};
template class __EXPORT BlockParam<float>;
template class __EXPORT BlockParam<int>;
} // namespace control
src/modules/controllib/block/BlockParam.hpp
-94
View File
@@ -1,94 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file BlockParam.h
*
* Controller library code
*/
#pragma once
#include <systemlib/param/param.h>
#include "Block.hpp"
#include <containers/List.hpp>
namespace control
{
class Block;
/**
* A base class for block params that enables traversing linked list.
*/
class __EXPORT BlockParamBase : public ListNode<BlockParamBase *>
{
public:
/**
* Instantiate a block param base.
*
* @param parent_prefix Set to true to include the parent name in the parameter name
*/
BlockParamBase(Block *parent, const char *name, bool parent_prefix = true);
virtual ~BlockParamBase() {};
virtual void update() = 0;
const char *getName() { return param_name(_handle); }
protected:
param_t _handle;
};
/**
* Parameters that are tied to blocks for updating and nameing.
*/
template <class T>
class BlockParam : public BlockParamBase
{
public:
BlockParam(Block *block, const char *name,
bool parent_prefix = true, T *extern_address = NULL);
T get();
void commit();
void set(T val);
void update();
virtual ~BlockParam();
protected:
T _val;
T *_extern_address;
};
typedef BlockParam<float> BlockParamFloat;
typedef BlockParam<int> BlockParamInt;
} // namespace control
src/modules/controllib/blocks.hpp
-664
View File
@@ -1,664 +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.h
*
* Controller library code
*/
#pragma once
#include <px4_defines.h>
#include <assert.h>
#include <time.h>
#include <stdlib.h>
#include <math.h>
#include <mathlib/math/test/test.hpp>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include "block/Block.hpp"
#include "block/BlockParam.hpp"
#include "matrix/math.hpp"
namespace control
{
int __EXPORT basicBlocksTest();
/**
* A limiter/ saturation.
* The output of update is the input, bounded
* by min/max.
*/
class __EXPORT BlockLimit : public Block
{
public:
// methods
BlockLimit(SuperBlock *parent, const char *name) :
Block(parent, name),
_min(this, "MIN"),
_max(this, "MAX")
{};
virtual ~BlockLimit() {};
float update(float input);
// accessors
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
protected:
// attributes
control::BlockParamFloat _min;
control::BlockParamFloat _max;
};
int __EXPORT blockLimitTest();
/**
* A symmetric limiter/ saturation.
* Same as limiter but with only a max, is used for
* upper limit of +max, and lower limit of -max
*/
class __EXPORT BlockLimitSym : public Block
{
public:
// methods
BlockLimitSym(SuperBlock *parent, const char *name) :
Block(parent, name),
_max(this, "MAX")
{};
virtual ~BlockLimitSym() {};
float update(float input);
// accessors
float getMax() { return _max.get(); }
protected:
// attributes
control::BlockParamFloat _max;
};
int __EXPORT blockLimitSymTest();
/**
* A low pass filter as described here:
* http://en.wikipedia.org/wiki/Low-pass_filter.
*/
class __EXPORT BlockLowPass : public Block
{
public:
// methods
BlockLowPass(SuperBlock *parent, const char *name) :
Block(parent, name),
_state(0.0f / 0.0f /* initialize to invalid val, force into is_finite() check on first call */),
_fCut(this, "") // only one parameter, no need to name
{};
virtual ~BlockLowPass() {};
float update(float input);
// accessors
float getState() { return _state; }
float getFCut() { return _fCut.get(); }
void setState(float state) { _state = state; }
protected:
// attributes
float _state;
control::BlockParamFloat _fCut;
};
int __EXPORT blockLowPassTest();
/**
* A high pass filter as described here:
* http://en.wikipedia.org/wiki/High-pass_filter.
*/
class __EXPORT BlockHighPass : public Block
{
public:
// methods
BlockHighPass(SuperBlock *parent, const char *name) :
Block(parent, name),
_u(0),
_y(0),
_fCut(this, "") // only one parameter, no need to name
{};
virtual ~BlockHighPass() {};
float update(float input);
// accessors
float getU() {return _u;}
float getY() {return _y;}
float getFCut() {return _fCut.get();}
void setU(float u) {_u = u;}
void setY(float y) {_y = y;}
protected:
// attributes
float _u; /**< previous input */
float _y; /**< previous output */
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
*/
class __EXPORT BlockLowPass2 : public Block
{
public:
// methods
BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) :
Block(parent, name),
_state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */),
_fCut(this, ""), // only one parameter, no need to name
_fs(sample_freq),
_lp(_fs, _fCut.get())
{};
virtual ~BlockLowPass2() {};
float update(float input);
// accessors
float getState() { return _state; }
float getFCutParam() { return _fCut.get(); }
void setState(float state) { _state = _lp.reset(state); }
protected:
// attributes
float _state;
control::BlockParamFloat _fCut;
float _fs;
math::LowPassFilter2p _lp;
};
int __EXPORT blockLowPass2Test();
/**
* A rectangular integrator.
* A limiter is built into the class to bound the
* integral's internal state. This is important
* for windup protection.
* @see Limit
*/
class __EXPORT BlockIntegral: public SuperBlock
{
public:
// methods
BlockIntegral(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_y(0),
_limit(this, "") {};
virtual ~BlockIntegral() {};
float update(float input);
// accessors
float getY() {return _y;}
float getMax() {return _limit.getMax();}
void setY(float y) {_y = y;}
protected:
// attributes
float _y; /**< previous output */
BlockLimitSym _limit; /**< limiter */
};
int __EXPORT blockIntegralTest();
/**
* A trapezoidal integrator.
* http://en.wikipedia.org/wiki/Trapezoidal_rule
* A limiter is built into the class to bound the
* integral's internal state. This is important
* for windup protection.
* @see Limit
*/
class __EXPORT BlockIntegralTrap : public SuperBlock
{
public:
// methods
BlockIntegralTrap(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_y(0),
_limit(this, "") {};
virtual ~BlockIntegralTrap() {};
float update(float input);
// accessors
float getU() {return _u;}
float getY() {return _y;}
float getMax() {return _limit.getMax();}
void setU(float u) {_u = u;}
void setY(float y) {_y = y;}
protected:
// attributes
float _u; /**< previous input */
float _y; /**< previous output */
BlockLimitSym _limit; /**< limiter */
};
int __EXPORT blockIntegralTrapTest();
/**
* A simple derivative approximation.
* This uses the previous and current input.
* This has a built in low pass filter.
* @see LowPass
*/
class __EXPORT BlockDerivative : public SuperBlock
{
public:
// methods
BlockDerivative(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_initialized(false),
_lowPass(this, "LP")
{};
virtual ~BlockDerivative() {};
/**
* Update the state and get current derivative
*
* This call updates the state and gets the current
* derivative. As the derivative is only valid
* on the second call to update, it will return
* no change (0) on the first. To get a closer
* estimate of the derivative on the first call,
* call setU() one time step before using the
* return value of update().
*
* @param input the variable to calculate the derivative of
* @return the current derivative
*/
float update(float input);
// accessors
void setU(float u) {_u = u;}
float getU() {return _u;}
float getLP() {return _lowPass.getFCut();}
float getO() { return _lowPass.getState(); }
protected:
// attributes
float _u; /**< previous input */
bool _initialized;
BlockLowPass _lowPass; /**< low pass filter */
};
int __EXPORT blockDerivativeTest();
/**
* A proportional controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockP: public Block
{
public:
// methods
BlockP(SuperBlock *parent, const char *name) :
Block(parent, name),
_kP(this, "") // only one param, no need to name
{};
virtual ~BlockP() {};
float update(float input)
{
return getKP() * input;
}
// accessors
float getKP() { return _kP.get(); }
protected:
control::BlockParamFloat _kP;
};
int __EXPORT blockPTest();
/**
* A proportional-integral controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockPI: public SuperBlock
{
public:
// methods
BlockPI(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_integral(this, "I"),
_kP(this, "P"),
_kI(this, "I")
{};
virtual ~BlockPI() {};
float update(float input)
{
return getKP() * input +
getKI() * getIntegral().update(input);
}
// accessors
float getKP() { return _kP.get(); }
float getKI() { return _kI.get(); }
BlockIntegral &getIntegral() { return _integral; }
private:
BlockIntegral _integral;
control::BlockParamFloat _kP;
control::BlockParamFloat _kI;
};
int __EXPORT blockPITest();
/**
* A proportional-derivative controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockPD: public SuperBlock
{
public:
// methods
BlockPD(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_derivative(this, "D"),
_kP(this, "P"),
_kD(this, "D")
{};
virtual ~BlockPD() {};
float update(float input)
{
return getKP() * input +
getKD() * getDerivative().update(input);
}
// accessors
float getKP() { return _kP.get(); }
float getKD() { return _kD.get(); }
BlockDerivative &getDerivative() { return _derivative; }
private:
BlockDerivative _derivative;
control::BlockParamFloat _kP;
control::BlockParamFloat _kD;
};
int __EXPORT blockPDTest();
/**
* A proportional-integral-derivative controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockPID: public SuperBlock
{
public:
// methods
BlockPID(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_integral(this, "I"),
_derivative(this, "D"),
_kP(this, "P"),
_kI(this, "I"),
_kD(this, "D")
{};
virtual ~BlockPID() {};
float update(float input)
{
return getKP() * input +
getKI() * getIntegral().update(input) +
getKD() * getDerivative().update(input);
}
// accessors
float getKP() { return _kP.get(); }
float getKI() { return _kI.get(); }
float getKD() { return _kD.get(); }
BlockIntegral &getIntegral() { return _integral; }
BlockDerivative &getDerivative() { return _derivative; }
private:
// attributes
BlockIntegral _integral;
BlockDerivative _derivative;
control::BlockParamFloat _kP;
control::BlockParamFloat _kI;
control::BlockParamFloat _kD;
};
int __EXPORT blockPIDTest();
/**
* An output trim/ saturation block
*/
class __EXPORT BlockOutput: public SuperBlock
{
public:
// methods
BlockOutput(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_trim(this, "TRIM"),
_limit(this, ""),
_val(0)
{
update(0);
};
virtual ~BlockOutput() {};
void update(float input)
{
_val = _limit.update(input + getTrim());
}
// accessors
float getMin() { return _limit.getMin(); }
float getMax() { return _limit.getMax(); }
float getTrim() { return _trim.get(); }
float get() { return _val; }
private:
// attributes
control::BlockParamFloat _trim;
BlockLimit _limit;
float _val;
};
int __EXPORT blockOutputTest();
/**
* A uniform random number generator
*/
class __EXPORT BlockRandUniform: public Block
{
public:
// methods
BlockRandUniform(SuperBlock *parent,
const char *name) :
Block(parent, name),
_min(this, "MIN"),
_max(this, "MAX")
{
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandUniform() {};
float update()
{
static float rand_max = MAX_RAND;
float rand_val = rand();
float bounds = getMax() - getMin();
return getMin() + (rand_val * bounds) / rand_max;
}
// accessors
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
private:
// attributes
control::BlockParamFloat _min;
control::BlockParamFloat _max;
};
int __EXPORT blockRandUniformTest();
class __EXPORT BlockRandGauss: public Block
{
public:
// methods
BlockRandGauss(SuperBlock *parent,
const char *name) :
Block(parent, name),
_mean(this, "MEAN"),
_stdDev(this, "DEV")
{
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandGauss() {};
float update()
{
static float V1, V2, S;
static int phase = 0;
float X;
if (phase == 0) {
do {
float U1 = (float)rand() / MAX_RAND;
float U2 = (float)rand() / MAX_RAND;
V1 = 2 * U1 - 1;
V2 = 2 * U2 - 1;
S = V1 * V1 + V2 * V2;
} while (S >= 1 || fabsf(S) < 1e-8f);
X = V1 * float(sqrt(-2 * float(log(S)) / S));
} else {
X = V2 * float(sqrt(-2 * float(log(S)) / S));
}
phase = 1 - phase;
return X * getStdDev() + getMean();
}
// accessors
float getMean() { return _mean.get(); }
float getStdDev() { return _stdDev.get(); }
private:
// attributes
control::BlockParamFloat _mean;
control::BlockParamFloat _stdDev;
};
int __EXPORT blockRandGaussTest();
template<class Type, size_t M>
class __EXPORT BlockStats: public Block
{
public:
// methods
BlockStats(SuperBlock *parent,
const char *name) :
Block(parent, name),
_sum(),
_sumSq(),
_count(0)
{
};
virtual ~BlockStats() {};
void update(const matrix::Vector<Type, M> &u)
{
_sum += u;
_sumSq += u.emult(u);
_count += 1;
}
void reset()
{
_sum.setZero();
_sumSq.setZero();
_count = 0;
}
// accessors
size_t getCount() { return _count; }
matrix::Vector<Type, M> getMean() { return _sum / _count; }
matrix::Vector<Type, M> getVar()
{
return (_sumSq - _sum.emult(_sum) / _count) / _count;
}
matrix::Vector<Type, M> getStdDev()
{
return getVar().pow(0.5);
}
private:
// attributes
matrix::Vector<Type, M> _sum;
matrix::Vector<Type, M> _sumSq;
size_t _count;
};
int __EXPORT blockStatsTest();
template<class Type, size_t M, size_t N, size_t LEN>
class __EXPORT BlockDelay: public Block
{
public:
// methods
BlockDelay(SuperBlock *parent,
const char *name) :
Block(parent, name),
_h(),
_index(0),
_delay(-1)
{
};
virtual ~BlockDelay() {};
matrix::Matrix<Type, M, N> update(const matrix::Matrix<Type, M, N> &u)
{
// store current value
_h[_index] = u;
// delay starts at zero, then increases to LEN
_delay += 1;
if (_delay > (LEN - 1)) {
_delay = LEN - 1;
}
// compute position of delayed value
int j = _index - _delay;
if (j < 0) {
j += LEN;
}
// increment storage position
_index += 1;
if (_index > (LEN - 1)) {
_index = 0;
}
// get delayed value
return _h[j];
}
matrix::Matrix<Type, M, N> get(size_t delay)
{
int j = _index - delay;
if (j < 0) { j += LEN; }
return _h[j];
}
private:
// attributes
matrix::Matrix<Type, M, N> _h[LEN];
size_t _index;
int _delay;
};
int __EXPORT blockDelayTest();
} // namespace control
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/uorb/blocks.cpp
-103
View File
@@ -1,103 +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 uorb_blocks.cpp
*
* uorb block library code
*/
#include "blocks.hpp"
#include <geo/geo.h>
namespace control
{
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_xtYawLimit(this, "XT2YAW"),
_xt2Yaw(this, "XT2YAW"),
_psiCmd(0)
{
}
BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(
const vehicle_global_position_s &pos,
const vehicle_attitude_s &att,
const position_setpoint_s &missionCmd,
const position_setpoint_s &lastMissionCmd)
{
// heading to waypoint
float psiTrack = get_bearing_to_next_waypoint(
(double)pos.lat / (double)1e7,
(double)pos.lon / (double)1e7,
missionCmd.lat,
missionCmd.lon);
// cross track
struct crosstrack_error_s xtrackError;
get_distance_to_line(&xtrackError,
(double)pos.lat / (double)1e7,
(double)pos.lon / (double)1e7,
lastMissionCmd.lat,
lastMissionCmd.lon,
missionCmd.lat,
missionCmd.lon);
_psiCmd = _wrap_2pi(psiTrack -
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
}
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
// publications
_actuators(ORB_ID(actuator_controls_0), -1, &getPublications())
{
}
BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
} // namespace control
src/modules/controllib/uorb/blocks.hpp
-113
View File
@@ -1,113 +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 uorb_blocks.h
*
* uorb block library code
*/
#pragma once
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/parameter_update.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include "../blocks.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
namespace control
{
/**
* Waypoint Guidance block
*/
class __EXPORT BlockWaypointGuidance : public SuperBlock
{
private:
BlockLimitSym _xtYawLimit;
BlockP _xt2Yaw;
float _psiCmd;
public:
BlockWaypointGuidance(SuperBlock *parent, const char *name);
virtual ~BlockWaypointGuidance();
void update(const vehicle_global_position_s &pos,
const vehicle_attitude_s &att,
const position_setpoint_s &missionCmd,
const position_setpoint_s &lastMissionCmd);
float getPsiCmd() { return _psiCmd; }
};
/**
* UorbEnabledAutopilot
*/
class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
{
protected:
// subscriptions
uORB::Subscription<vehicle_attitude_s> _att;
uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
uORB::Subscription<vehicle_global_position_s> _pos;
uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
uORB::Subscription<manual_control_setpoint_s> _manual;
uORB::Subscription<vehicle_status_s> _status;
uORB::Subscription<parameter_update_s> _param_update;
// publications
uORB::Publication<actuator_controls_s> _actuators;
public:
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
virtual ~BlockUorbEnabledAutopilot();
};
} // namespace control
src/modules/controllib/CMakeLists.txt → src/modules/controllib_test/CMakeLists.txt
+2 -5
View File
@@ -31,18 +31,15 @@
#
############################################################################
px4_add_module(
MODULE modules__controllib
MODULE modules__controllib_test
MAIN controllib_test
COMPILE_FLAGS
-Os
SRCS
controllib_test_main.cpp
test_params.c
block/Block.cpp
block/BlockParam.cpp
uorb/blocks.cpp
blocks.cpp
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
@@ -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;
}
src/modules/controllib/test_params.c → src/modules/controllib_test/test_params.c
View File