mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-30 05:33:34 +08:00
Moved control library.
This commit is contained in:
James Goppert
@@ -0,0 +1,45 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE lib__controllib
|
||||
COMPILE_FLAGS
|
||||
-Os
|
||||
SRCS
|
||||
blocks.cpp
|
||||
block/Block.cpp
|
||||
block/BlockParam.cpp
|
||||
uorb/blocks.cpp
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
@@ -0,0 +1,216 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 *>;
|
||||
@@ -0,0 +1,143 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
@@ -0,0 +1,130 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
@@ -0,0 +1,94 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
@@ -0,0 +1,148 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file blocks.cpp
|
||||
*
|
||||
* Controller library code
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <float.h>
|
||||
|
||||
#include "blocks.hpp"
|
||||
|
||||
#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; }
|
||||
|
||||
namespace control
|
||||
{
|
||||
|
||||
float BlockLimit::update(float input)
|
||||
{
|
||||
if (input > getMax()) {
|
||||
input = _max.get();
|
||||
|
||||
} else if (input < getMin()) {
|
||||
input = getMin();
|
||||
}
|
||||
|
||||
return input;
|
||||
}
|
||||
|
||||
float BlockLimitSym::update(float input)
|
||||
{
|
||||
if (input > getMax()) {
|
||||
input = _max.get();
|
||||
|
||||
} else if (input < -getMax()) {
|
||||
input = -getMax();
|
||||
}
|
||||
|
||||
return input;
|
||||
}
|
||||
|
||||
float BlockLowPass::update(float input)
|
||||
{
|
||||
if (!PX4_ISFINITE(getState())) {
|
||||
setState(input);
|
||||
}
|
||||
|
||||
float b = 2 * float(M_PI) * getFCut() * getDt();
|
||||
float a = b / (1 + b);
|
||||
setState(a * input + (1 - a)*getState());
|
||||
return getState();
|
||||
}
|
||||
|
||||
float BlockHighPass::update(float input)
|
||||
{
|
||||
float b = 2 * float(M_PI) * getFCut() * getDt();
|
||||
float a = 1 / (1 + b);
|
||||
setY(a * (getY() + input - getU()));
|
||||
setU(input);
|
||||
return getY();
|
||||
}
|
||||
|
||||
float BlockLowPass2::update(float input)
|
||||
{
|
||||
if (!PX4_ISFINITE(getState())) {
|
||||
setState(input);
|
||||
}
|
||||
|
||||
if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
|
||||
_lp.set_cutoff_frequency(_fs, getFCutParam());
|
||||
}
|
||||
|
||||
_state = _lp.apply(input);
|
||||
return _state;
|
||||
}
|
||||
|
||||
float BlockIntegral::update(float input)
|
||||
{
|
||||
// trapezoidal integration
|
||||
setY(_limit.update(getY() + input * getDt()));
|
||||
return getY();
|
||||
}
|
||||
|
||||
float BlockIntegralTrap::update(float input)
|
||||
{
|
||||
// trapezoidal integration
|
||||
setY(_limit.update(getY() +
|
||||
(getU() + input) / 2.0f * getDt()));
|
||||
setU(input);
|
||||
return getY();
|
||||
}
|
||||
|
||||
float BlockDerivative::update(float input)
|
||||
{
|
||||
float output;
|
||||
|
||||
if (_initialized) {
|
||||
output = _lowPass.update((input - getU()) / getDt());
|
||||
|
||||
} else {
|
||||
// if this is the first call to update
|
||||
// we have no valid derivative
|
||||
// and so we use the assumption the
|
||||
// input value is not changing much,
|
||||
// which is the best we can do here.
|
||||
_lowPass.update(0.0f);
|
||||
output = 0.0f;
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
setU(input);
|
||||
return output;
|
||||
}
|
||||
|
||||
} // namespace control
|
||||
@@ -0,0 +1,628 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
{
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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 */
|
||||
};
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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 */
|
||||
};
|
||||
|
||||
/**
|
||||
* 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 */
|
||||
};
|
||||
|
||||
/**
|
||||
* 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 */
|
||||
};
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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;
|
||||
};
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
} // namespace control
|
||||
@@ -0,0 +1,103 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
|
||||
@@ -0,0 +1,113 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
|
||||
Reference in New Issue
Block a user