From 8888b73e160520e5b15e168998013f4a5f6e64c0 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 6 Jan 2013 15:41:23 -0500 Subject: Added control library. --- apps/systemlib/control/block/Block.cpp | 210 +++++++++ apps/systemlib/control/block/Block.hpp | 131 ++++++ apps/systemlib/control/block/BlockParam.cpp | 77 ++++ apps/systemlib/control/block/BlockParam.hpp | 85 ++++ apps/systemlib/control/block/List.hpp | 71 ++++ apps/systemlib/control/block/UOrbPublication.cpp | 39 ++ apps/systemlib/control/block/UOrbPublication.hpp | 118 ++++++ apps/systemlib/control/block/UOrbSubscription.cpp | 51 +++ apps/systemlib/control/block/UOrbSubscription.hpp | 137 ++++++ apps/systemlib/control/blocks.cpp | 486 +++++++++++++++++++++ apps/systemlib/control/blocks.hpp | 494 ++++++++++++++++++++++ apps/systemlib/control/fixedwing.cpp | 351 +++++++++++++++ apps/systemlib/control/fixedwing.hpp | 438 +++++++++++++++++++ apps/systemlib/control/test_params.c | 22 + 14 files changed, 2710 insertions(+) create mode 100644 apps/systemlib/control/block/Block.cpp create mode 100644 apps/systemlib/control/block/Block.hpp create mode 100644 apps/systemlib/control/block/BlockParam.cpp create mode 100644 apps/systemlib/control/block/BlockParam.hpp create mode 100644 apps/systemlib/control/block/List.hpp create mode 100644 apps/systemlib/control/block/UOrbPublication.cpp create mode 100644 apps/systemlib/control/block/UOrbPublication.hpp create mode 100644 apps/systemlib/control/block/UOrbSubscription.cpp create mode 100644 apps/systemlib/control/block/UOrbSubscription.hpp create mode 100644 apps/systemlib/control/blocks.cpp create mode 100644 apps/systemlib/control/blocks.hpp create mode 100644 apps/systemlib/control/fixedwing.cpp create mode 100644 apps/systemlib/control/fixedwing.hpp create mode 100644 apps/systemlib/control/test_params.c (limited to 'apps/systemlib/control') diff --git a/apps/systemlib/control/block/Block.cpp b/apps/systemlib/control/block/Block.cpp new file mode 100644 index 000000000..5994d2315 --- /dev/null +++ b/apps/systemlib/control/block/Block.cpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#include "Block.hpp" +#include "BlockParam.hpp" +#include "UOrbSubscription.hpp" +#include "UOrbPublication.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() +{ + UOrbSubscriptionBase *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() +{ + UOrbPublicationBase *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 diff --git a/apps/systemlib/control/block/Block.hpp b/apps/systemlib/control/block/Block.hpp new file mode 100644 index 000000000..258701f27 --- /dev/null +++ b/apps/systemlib/control/block/Block.hpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 + +#include +#include + +#include "List.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 UOrbSubscriptionBase; +class UOrbPublicationBase; +class SuperBlock; + +/** + */ +class __EXPORT Block : + public ListNode +{ +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 & getSubscriptions() { return _subscriptions; } + List & getPublications() { return _publications; } + List & getParams() { return _params; } +// attributes + const char *_name; + SuperBlock *_parent; + float _dt; + List _subscriptions; + List _publications; + List _params; +}; + +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 & getChildren() { return _children; } + void updateChildParams(); + void updateChildSubscriptions(); + void updateChildPublications(); +// attributes + List _children; +}; + +} // namespace control diff --git a/apps/systemlib/control/block/BlockParam.cpp b/apps/systemlib/control/block/BlockParam.cpp new file mode 100644 index 000000000..b3f49f7db --- /dev/null +++ b/apps/systemlib/control/block/BlockParam.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#include "BlockParam.hpp" + +namespace control +{ + +BlockParamBase::BlockParamBase(Block *parent, const char *name) : + _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 { + snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name); + } + + parent->getParams().add(this); + } + + _handle = param_find(fullname); + + if (_handle == PARAM_INVALID) + printf("error finding param: %s\n", fullname); +}; + +} // namespace control diff --git a/apps/systemlib/control/block/BlockParam.hpp b/apps/systemlib/control/block/BlockParam.hpp new file mode 100644 index 000000000..7f86d1717 --- /dev/null +++ b/apps/systemlib/control/block/BlockParam.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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 + +#include "Block.hpp" +#include "List.hpp" + +namespace control +{ + +/** + * A base class for block params that enables traversing linked list. + */ +class __EXPORT BlockParamBase : public ListNode +{ +public: + BlockParamBase(Block *parent, const char *name); + 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 __EXPORT BlockParam : public BlockParamBase +{ +public: + BlockParam(Block *block, const char *name) : + BlockParamBase(block, name), + _val() { + update(); + } + T get() { return _val; } + void set(T val) { _val = val; } + void update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); + } +protected: + T _val; +}; + +} // namespace control diff --git a/apps/systemlib/control/block/List.hpp b/apps/systemlib/control/block/List.hpp new file mode 100644 index 000000000..96b0b94d1 --- /dev/null +++ b/apps/systemlib/control/block/List.hpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 Node.h + * + * A node of a linked list. + */ + +#pragma once + +template +class __EXPORT ListNode +{ +public: + ListNode() : _sibling(NULL) { + } + void setSibling(T sibling) { _sibling = sibling; } + T getSibling() { return _sibling; } + T get() { + return _sibling; + } +protected: + T _sibling; +}; + +template +class __EXPORT List +{ +public: + List() : _head() { + } + void add(T newNode) { + newNode->setSibling(getHead()); + setHead(newNode); + } + T getHead() { return _head; } +private: + void setHead(T &head) { _head = head; } + T _head; +}; diff --git a/apps/systemlib/control/block/UOrbPublication.cpp b/apps/systemlib/control/block/UOrbPublication.cpp new file mode 100644 index 000000000..f69b39d90 --- /dev/null +++ b/apps/systemlib/control/block/UOrbPublication.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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 UOrbPublication.cpp + * + */ + +#include "UOrbPublication.hpp" diff --git a/apps/systemlib/control/block/UOrbPublication.hpp b/apps/systemlib/control/block/UOrbPublication.hpp new file mode 100644 index 000000000..a36f4429f --- /dev/null +++ b/apps/systemlib/control/block/UOrbPublication.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 UOrbPublication.h + * + */ + +#pragma once + +#include +#include "Block.hpp" +#include "List.hpp" + + +namespace control +{ + +class Block; + +/** + * Base publication warapper class, used in list traversal + * of various publications. + */ +class __EXPORT UOrbPublicationBase : public ListNode +{ +public: + + UOrbPublicationBase( + List * list, + const struct orb_metadata *meta) : + _meta(meta), + _handle() { + if (list != NULL) list->add(this); + } + void update() { + orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + } + virtual void *getDataVoidPtr() = 0; + virtual ~UOrbPublicationBase() { + orb_unsubscribe(getHandle()); + } + const struct orb_metadata *getMeta() { return _meta; } + int getHandle() { return _handle; } +protected: + void setHandle(orb_advert_t handle) { _handle = handle; } + const struct orb_metadata *_meta; + orb_advert_t _handle; +}; + +/** + * UOrb Publication wrapper class + */ +template +class UOrbPublication : + public T, // this must be first! + public UOrbPublicationBase +{ +public: + /** + * Constructor + * + * @param list A list interface for adding to list during construction + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + */ + UOrbPublication( + List * list, + const struct orb_metadata *meta) : + T(), // initialize data structure to zero + UOrbPublicationBase(list, meta) { + // It is important that we call T() + // before we publish the data, so we + // call this here instead of the base class + setHandle(orb_advertise(getMeta(), getDataVoidPtr())); + } + virtual ~UOrbPublication() {} + /* + * XXX + * This function gets the T struct, assuming + * the struct is the first base class, this + * should use dynamic cast, but doesn't + * seem to be available + */ + void *getDataVoidPtr() { return (void *)(T *)(this); } +}; + +} // namespace control diff --git a/apps/systemlib/control/block/UOrbSubscription.cpp b/apps/systemlib/control/block/UOrbSubscription.cpp new file mode 100644 index 000000000..022cadd24 --- /dev/null +++ b/apps/systemlib/control/block/UOrbSubscription.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 UOrbSubscription.cpp + * + */ + +#include "UOrbSubscription.hpp" + +namespace control +{ + +bool __EXPORT UOrbSubscriptionBase::updated() +{ + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; +} + +} // namespace control diff --git a/apps/systemlib/control/block/UOrbSubscription.hpp b/apps/systemlib/control/block/UOrbSubscription.hpp new file mode 100644 index 000000000..22cc2e114 --- /dev/null +++ b/apps/systemlib/control/block/UOrbSubscription.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * 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 UOrbSubscription.h + * + */ + +#pragma once + +#include +#include "Block.hpp" +#include "List.hpp" + + +namespace control +{ + +class Block; + +/** + * Base subscription warapper class, used in list traversal + * of various subscriptions. + */ +class __EXPORT UOrbSubscriptionBase : + public ListNode +{ +public: +// methods + + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + */ + UOrbSubscriptionBase( + List * list, + const struct orb_metadata *meta) : + _meta(meta), + _handle() { + if (list != NULL) list->add(this); + } + bool updated(); + void update() { + if (updated()) { + orb_copy(_meta, _handle, getDataVoidPtr()); + } + } + virtual void *getDataVoidPtr() = 0; + virtual ~UOrbSubscriptionBase() { + orb_unsubscribe(_handle); + } +// accessors + const struct orb_metadata *getMeta() { return _meta; } + int getHandle() { return _handle; } +protected: +// accessors + void setHandle(int handle) { _handle = handle; } +// attributes + const struct orb_metadata *_meta; + int _handle; +}; + +/** + * UOrb Subscription wrapper class + */ +template +class __EXPORT UOrbSubscription : + public T, // this must be first! + public UOrbSubscriptionBase +{ +public: + /** + * Constructor + * + * @param list A list interface for adding to list during construction + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param interval The minimum interval in milliseconds between updates + */ + UOrbSubscription( + List * list, + const struct orb_metadata *meta, unsigned interval) : + T(), // initialize data structure to zero + UOrbSubscriptionBase(list, meta) { + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); + } + + /** + * Deconstructor + */ + virtual ~UOrbSubscription() {} + + /* + * XXX + * This function gets the T struct, assuming + * the struct is the first base class, this + * should use dynamic cast, but doesn't + * seem to be available + */ + void *getDataVoidPtr() { return (void *)(T *)(this); } + T getData() { return T(*this); } +}; + +} // namespace control diff --git a/apps/systemlib/control/blocks.cpp b/apps/systemlib/control/blocks.cpp new file mode 100644 index 000000000..c6c374300 --- /dev/null +++ b/apps/systemlib/control/blocks.cpp @@ -0,0 +1,486 @@ +/**************************************************************************** + * + * 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 +#include + +#include "blocks.hpp" + +namespace control +{ + +int basicBlocksTest() +{ + blockLimitTest(); + blockLimitSymTest(); + blockLowPassTest(); + blockHighPassTest(); + blockIntegralTest(); + blockIntegralTrapTest(); + blockDerivativeTest(); + blockPTest(); + blockPITest(); + blockPDTest(); + blockPIDTest(); + blockOutputTest(); + blockRandUniformTest(); + blockRandGaussTest(); + 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: "); + BlockLimit limit(NULL, "TEST"); + // initial state + ASSERT(equal(1.0f, limit.getMax())); + ASSERT(equal(-1.0f, limit.getMin())); + ASSERT(equal(0.0f, limit.getDt())); + // update + ASSERT(equal(-1.0f, limit.update(-2.0f))); + ASSERT(equal(1.0f, limit.update(2.0f))); + ASSERT(equal(0.0f, limit.update(0.0f))); + printf("PASS\n"); + 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: "); + BlockLimitSym limit(NULL, "TEST"); + // initial state + ASSERT(equal(1.0f, limit.getMax())); + ASSERT(equal(0.0f, limit.getDt())); + // update + ASSERT(equal(-1.0f, limit.update(-2.0f))); + ASSERT(equal(1.0f, limit.update(2.0f))); + ASSERT(equal(0.0f, limit.update(0.0f))); + printf("PASS\n"); + return 0; +} + +float BlockLowPass::update(float 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: "); + BlockLowPass lowPass(NULL, "TEST_LP"); + // test initial state + ASSERT(equal(10.0f, lowPass.getFCut())); + ASSERT(equal(0.0f, lowPass.getState())); + ASSERT(equal(0.0f, lowPass.getDt())); + // set dt + lowPass.setDt(0.1f); + ASSERT(equal(0.1f, lowPass.getDt())); + // set state + lowPass.setState(1.0f); + ASSERT(equal(1.0f, lowPass.getState())); + // test update + ASSERT(equal(1.8626974f, lowPass.update(2.0f))); + + // test end condition + for (int i = 0; i < 100; i++) { + lowPass.update(2.0f); + } + + ASSERT(equal(2.0f, lowPass.getState())); + ASSERT(equal(2.0f, lowPass.update(2.0f))); + printf("PASS\n"); + 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: "); + BlockHighPass highPass(NULL, "TEST_HP"); + // test initial state + ASSERT(equal(10.0f, highPass.getFCut())); + ASSERT(equal(0.0f, highPass.getU())); + ASSERT(equal(0.0f, highPass.getY())); + ASSERT(equal(0.0f, highPass.getDt())); + // set dt + highPass.setDt(0.1f); + ASSERT(equal(0.1f, highPass.getDt())); + // set state + highPass.setU(1.0f); + ASSERT(equal(1.0f, highPass.getU())); + highPass.setY(1.0f); + ASSERT(equal(1.0f, highPass.getY())); + // test update + ASSERT(equal(0.2746051f, highPass.update(2.0f))); + + // test end condition + for (int i = 0; i < 100; i++) { + highPass.update(2.0f); + } + + ASSERT(equal(0.0f, highPass.getY())); + ASSERT(equal(0.0f, highPass.update(2.0f))); + printf("PASS\n"); + return 0; +} + +float BlockIntegral::update(float input) +{ + // trapezoidal integration + setY(_limit.update(getY() + input * getDt())); + return getY(); +} + +int blockIntegralTest() +{ + printf("Test BlockIntegral\t\t: "); + BlockIntegral integral(NULL, "TEST_I"); + // test initial state + ASSERT(equal(1.0f, integral.getMax())); + ASSERT(equal(0.0f, integral.getDt())); + // set dt + integral.setDt(0.1f); + ASSERT(equal(0.1f, integral.getDt())); + // set Y + integral.setY(0.9f); + ASSERT(equal(0.9f, integral.getY())); + + // test exceed max + for (int i = 0; i < 100; i++) { + integral.update(1.0f); + } + + ASSERT(equal(1.0f, integral.update(1.0f))); + // test exceed min + integral.setY(-0.9f); + ASSERT(equal(-0.9f, integral.getY())); + + for (int i = 0; i < 100; i++) { + integral.update(-1.0f); + } + + ASSERT(equal(-1.0f, integral.update(-1.0f))); + // test update + integral.setY(0.1f); + ASSERT(equal(0.2f, integral.update(1.0))); + ASSERT(equal(0.2f, integral.getY())); + printf("PASS\n"); + 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: "); + BlockIntegralTrap integral(NULL, "TEST_I"); + // test initial state + ASSERT(equal(1.0f, integral.getMax())); + ASSERT(equal(0.0f, integral.getDt())); + // set dt + integral.setDt(0.1f); + ASSERT(equal(0.1f, integral.getDt())); + // set U + integral.setU(1.0f); + ASSERT(equal(1.0f, integral.getU())); + // set Y + integral.setY(0.9f); + ASSERT(equal(0.9f, integral.getY())); + + // test exceed max + for (int i = 0; i < 100; i++) { + integral.update(1.0f); + } + + ASSERT(equal(1.0f, integral.update(1.0f))); + // test exceed min + integral.setU(-1.0f); + integral.setY(-0.9f); + ASSERT(equal(-0.9f, integral.getY())); + + for (int i = 0; i < 100; i++) { + integral.update(-1.0f); + } + + ASSERT(equal(-1.0f, integral.update(-1.0f))); + // test update + integral.setU(2.0f); + integral.setY(0.1f); + ASSERT(equal(0.25f, integral.update(1.0))); + ASSERT(equal(0.25f, integral.getY())); + ASSERT(equal(1.0f, integral.getU())); + printf("PASS\n"); + return 0; +} + +float BlockDerivative::update(float input) +{ + float output = _lowPass.update((input - getU()) / getDt()); + setU(input); + return output; +} + +int blockDerivativeTest() +{ + printf("Test BlockDerivative\t\t: "); + BlockDerivative derivative(NULL, "TEST_D"); + // test initial state + ASSERT(equal(0.0f, derivative.getU())); + ASSERT(equal(10.0f, derivative.getLP())); + // set dt + derivative.setDt(0.1f); + ASSERT(equal(0.1f, derivative.getDt())); + // set U + derivative.setU(1.0f); + ASSERT(equal(1.0f, derivative.getU())); + // test update + ASSERT(equal(8.6269744f, derivative.update(2.0f))); + ASSERT(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(equal(0.2f, blockP.getKP())); + ASSERT(equal(0.0f, blockP.getDt())); + // set dt + blockP.setDt(0.1f); + ASSERT(equal(0.1f, blockP.getDt())); + // test update + ASSERT(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(equal(0.2f, blockPI.getKP())); + ASSERT(equal(0.1f, blockPI.getKI())); + ASSERT(equal(0.0f, blockPI.getDt())); + ASSERT(equal(1.0f, blockPI.getIntegral().getMax())); + // set dt + blockPI.setDt(0.1f); + ASSERT(equal(0.1f, blockPI.getDt())); + // set integral state + blockPI.getIntegral().setY(0.1f); + ASSERT(equal(0.1f, blockPI.getIntegral().getY())); + // test update + // 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43 + ASSERT(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(equal(0.2f, blockPD.getKP())); + ASSERT(equal(0.01f, blockPD.getKD())); + ASSERT(equal(0.0f, blockPD.getDt())); + ASSERT(equal(10.0f, blockPD.getDerivative().getLP())); + // set dt + blockPD.setDt(0.1f); + ASSERT(equal(0.1f, blockPD.getDt())); + // set derivative state + blockPD.getDerivative().setU(1.0f); + ASSERT(equal(1.0f, blockPD.getDerivative().getU())); + // test update + // 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744 + ASSERT(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(equal(0.2f, blockPID.getKP())); + ASSERT(equal(0.1f, blockPID.getKI())); + ASSERT(equal(0.01f, blockPID.getKD())); + ASSERT(equal(0.0f, blockPID.getDt())); + ASSERT(equal(10.0f, blockPID.getDerivative().getLP())); + ASSERT(equal(1.0f, blockPID.getIntegral().getMax())); + // set dt + blockPID.setDt(0.1f); + ASSERT(equal(0.1f, blockPID.getDt())); + // set derivative state + blockPID.getDerivative().setU(1.0f); + ASSERT(equal(1.0f, blockPID.getDerivative().getU())); + // set integral state + blockPID.getIntegral().setY(0.1f); + ASSERT(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(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(equal(0.0f, blockOutput.getDt())); + ASSERT(equal(0.5f, blockOutput.get())); + ASSERT(equal(-1.0f, blockOutput.getMin())); + ASSERT(equal(1.0f, blockOutput.getMax())); + // test update below min + blockOutput.update(-2.0f); + ASSERT(equal(-1.0f, blockOutput.get())); + // test update above max + blockOutput.update(2.0f); + ASSERT(equal(1.0f, blockOutput.get())); + // test trim + blockOutput.update(0.0f); + ASSERT(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(equal(0.0f, blockRandUniform.getDt())); + ASSERT(equal(-1.0f, blockRandUniform.getMin())); + ASSERT(equal(1.0f, blockRandUniform.getMax())); + // test update + int n = 10000; + float mean = blockRandUniform.update(); + + // recursive mean algorithm from Knuth + for (int i = 2; i < n + 1; i++) { + float val = blockRandUniform.update(); + mean += (val - mean) / i; + ASSERT(val <= blockRandUniform.getMax()); + ASSERT(val >= blockRandUniform.getMin()); + } + + ASSERT(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(equal(0.0f, blockRandGauss.getDt())); + ASSERT(equal(1.0f, blockRandGauss.getMean())); + ASSERT(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)); + ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); + ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); + printf("PASS\n"); + return 0; +} + +} // namespace control diff --git a/apps/systemlib/control/blocks.hpp b/apps/systemlib/control/blocks.hpp new file mode 100644 index 000000000..95f2021ce --- /dev/null +++ b/apps/systemlib/control/blocks.hpp @@ -0,0 +1,494 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.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 + BlockParam _min; + BlockParam _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 + BlockParam _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), + _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; + BlockParam _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 */ + BlockParam _fCut; /**< cut-off frequency, Hz */ +}; + +int __EXPORT blockHighPassTest(); + +/** + * 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), + _lowPass(this, "LP") + {}; + virtual ~BlockDerivative() {}; + float update(float input); +// accessors + void setU(float u) {_u = u;} + float getU() {return _u;} + float getLP() {return _lowPass.getFCut();} +protected: +// attributes + float _u; /**< previous input */ + 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: + BlockParam _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; + BlockParam _kP; + BlockParam _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; + BlockParam _kP; + BlockParam _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; + BlockParam _kP; + BlockParam _kI; + BlockParam _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 + BlockParam _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 + BlockParam _min; + BlockParam _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 + BlockParam _mean; + BlockParam _stdDev; +}; + +int __EXPORT blockRandGaussTest(); + +} // namespace control diff --git a/apps/systemlib/control/fixedwing.cpp b/apps/systemlib/control/fixedwing.cpp new file mode 100644 index 000000000..695653714 --- /dev/null +++ b/apps/systemlib/control/fixedwing.cpp @@ -0,0 +1,351 @@ +/**************************************************************************** + * + * 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 fixedwing.cpp + * + * Controller library code + */ + +#include "fixedwing.hpp" + +namespace control +{ + +namespace fixedwing +{ + +BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _rLowPass(this, "R_LP"), + _rWashout(this, "R_HP"), + _r2Rdr(this, "R2RDR"), + _rudder(0) +{ +} + +BlockYawDamper::~BlockYawDamper() {}; + +void BlockYawDamper::update(float rCmd, float r) +{ + _rudder = _r2Rdr.update(rCmd - + _rWashout.update(_rLowPass.update(r))); +} + +BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _yawDamper(this, ""), + _pLowPass(this, "P_LP"), + _qLowPass(this, "Q_LP"), + _p2Ail(this, "P2AIL"), + _q2Elv(this, "Q2ELV"), + _aileron(0), + _elevator(0) +{ +} + +BlockStabilization::~BlockStabilization() {}; + +void BlockStabilization::update(float pCmd, float qCmd, float rCmd, + float p, float q, float r) +{ + _aileron = _p2Ail.update( + pCmd - _pLowPass.update(p)); + _elevator = _q2Elv.update( + qCmd - _qLowPass.update(q)); + _yawDamper.update(rCmd, r); +} + +BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _psi2Phi(this, "PSI2PHI"), + _phi2P(this, "PHI2P"), + _phiLimit(this, "PHI_LIM") +{ +} + +BlockHeadingHold::~BlockHeadingHold() {}; + +float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p) +{ + float psiError = _wrap_pi(psiCmd - psi); + float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); + return _phi2P.update(phiCmd - phi); +} + +BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _v2Theta(this, "V2THE"), + _theta2Q(this, "THE2Q"), + _theLimit(this, "THE"), + _vLimit(this, "V") +{ +} + +BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {}; + +float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q) +{ + // negative sign because nose over to increase speed + float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v)); + return _theta2Q.update(thetaCmd - theta); +} + +BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _v2Thr(this, "V2THR") +{ +} + +BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {}; + +float BlockVelocityHoldFrontside::update(float vCmd, float v) +{ + return _v2Thr.update(vCmd - v); +} + +BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _h2Thr(this, "H2THR"), + _throttle(0) +{ +} + +BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {}; + +void BlockAltitudeHoldBackside::update(float hCmd, float h) +{ + _throttle = _h2Thr.update(hCmd - h); +} + +BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _h2Theta(this, "H2THE"), + _theta2Q(this, "THE2Q") +{ +} + +BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {}; + +float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q) +{ + float thetaCmd = _h2Theta.update(hCmd - h); + return _theta2Q.update(thetaCmd - theta); +} + +BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent, + const char *name, + BlockStabilization *stabilization) : + SuperBlock(parent, name), + _stabilization(stabilization), + _headingHold(this, ""), + _velocityHold(this, ""), + _altitudeHold(this, ""), + _trimAil(this, "TRIM_AIL"), + _trimElv(this, "TRIM_ELV"), + _trimRdr(this, "TRIM_RDR"), + _trimThr(this, "TRIM_THR") +{ +} + +BlockBacksideAutopilot::~BlockBacksideAutopilot() {}; + +void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd, + float h, float v, + float phi, float theta, float psi, + float p, float q, float r) +{ + _altitudeHold.update(hCmd, h); + _stabilization->update( + _headingHold.update(psiCmd, phi, psi, p), + _velocityHold.update(vCmd, v, theta, q), + rCmd, + p, q, r); +} + +BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _xtYawLimit(this, "XT2YAW"), + _xt2Yaw(this, "XT2YAW"), + _psiCmd(0) +{ +} + +BlockWaypointGuidance::~BlockWaypointGuidance() {}; + +void BlockWaypointGuidance::update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd) +{ + + // heading to waypoint + float psiTrack = get_bearing_to_next_waypoint( + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + // cross track + struct crosstrack_error_s xtrackError; + get_distance_to_line(&xtrackError, + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)lastPosCmd.lat / (double)1e7d, + (double)lastPosCmd.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + _psiCmd = _wrap_2pi(psiTrack - + _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); +} + +BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + // subscriptions + _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), + _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), + _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), + _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), + _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20), + _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), + _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), + _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + // publications + _actuators(&getPublications(), ORB_ID(actuator_controls_0)) +{ +} + +BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; + +BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : + BlockUorbEnabledAutopilot(parent, name), + _stabilization(this, ""), // no name needed, already unique + _backsideAutopilot(this, "", &_stabilization), + _guide(this, ""), + _vCmd(this, "V_CMD"), + _attPoll(), + _lastPosCmd(), + _timeStamp(0) +{ + _attPoll.fd = _att.getHandle(); + _attPoll.events = POLLIN; +} + +void BlockMultiModeBacksideAutopilot::update() +{ + // wait for a sensor update, check for exit condition every 100 ms + if (poll(&_attPoll, 1, 100) < 0) return; // poll error + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // check for sane values of dt + // to prevent large control responses + if (dt > 1.0f || dt < 0) return; + + // set dt for all child blocks + setDt(dt); + + // store old position command before update if new command sent + if (_posCmd.updated()) { + _lastPosCmd = _posCmd.getData(); + } + + // check for new updates + if (_param_update.updated()) updateParams(); + + // get new information from subscriptions + updateSubscriptions(); + + // default all output to zero unless handled by mode + for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + // handle autopilot modes + if (_status.state_machine == SYSTEM_STATE_STABILIZED) { + _stabilization.update( + _ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; + + } else if (_status.state_machine == SYSTEM_STATE_AUTO) { + // update guidance + _guide.update(_pos, _att, _posCmd, _lastPosCmd); + + // calculate velocity, XXX should be airspeed, but using ground speed for now + float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz); + + // commands + float rCmd = 0; + + _backsideAutopilot.update( + _posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(), + _pos.alt, v, + _att.roll, _att.pitch, _att.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed + ); + _actuators.control[CH_AIL] = _backsideAutopilot.getAileron(); + _actuators.control[CH_ELV] = _backsideAutopilot.getElevator(); + _actuators.control[CH_RDR] = _backsideAutopilot.getRudder(); + _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); + + } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; + } + + // update all publications + updatePublications(); +} + +BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() +{ + // send one last publication when destroyed, setting + // all output to zero + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + updatePublications(); +} + +} // namespace fixedwing + +} // namespace control + diff --git a/apps/systemlib/control/fixedwing.hpp b/apps/systemlib/control/fixedwing.hpp new file mode 100644 index 000000000..ea742641c --- /dev/null +++ b/apps/systemlib/control/fixedwing.hpp @@ -0,0 +1,438 @@ +/**************************************************************************** + * + * 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 fixedwing.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "blocks.hpp" +#include "block/UOrbSubscription.hpp" +#include "block/UOrbPublication.hpp" + +extern "C" { +#include +} + +namespace control +{ + +namespace fixedwing +{ + +/** + * BlockYawDamper + * + * This block has more explations to help new developers + * add their own blocks. It includes a limited explanation + * of some C++ basics. + * + * Block: The generic class describing a typical block as you + * would expect in Simulink or ScicosLab. A block can have + * parameters. It cannot have other blocks. + * + * SuperBlock: A superblock is a block that can have other + * blocks. It has methods that manage the blocks beneath it. + * + * BlockYawDamper inherits from SuperBlock publically, this + * means that any public function in SuperBlock are public within + * BlockYawDamper and may be called from outside the + * class methods. Any protected function within block + * are private to the class and may not be called from + * outside this class. Protected should be preferred + * where possible to public as it is important to + * limit access to the bare minimum to prevent + * accidental errors. + */ +class __EXPORT BlockYawDamper : public SuperBlock +{ +private: + /** + * Declaring other blocks used by this block + * + * In this section we declare all child blocks that + * this block is composed of. They are private + * members so only this block has direct access to + * them. + */ + BlockLowPass _rLowPass; + BlockHighPass _rWashout; + BlockP _r2Rdr; + + /** + * Declaring output values and accessors + * + * If we have any output values for the block we + * declare them here. Output can be directly returned + * through the update function, but outputs should be + * declared here if the information will likely be requested + * again, or if there are multiple outputs. + * + * You should only be able to set outputs from this block, + * so the outputs are declared in the private section. + * A get accessor is provided + * in the public section for other blocks to get the + * value of the output. + */ + float _rudder; +public: + /** + * BlockYawDamper Constructor + * + * The job of the constructor is to initialize all + * parameter in this block and initialize all child + * blocks. Note also, that the output values must be + * initialized here. The order of the members in the + * member initialization list should follow the + * order in which they are declared within the class. + * See the private member declarations above. + * + * Block Construction + * + * All blocks are constructed with their parent block + * and their name. This allows parameters within the + * block to construct a fully qualified name from + * concatenating the two. If the name provided to the + * block is "", then the block will use the parent + * name as it's name. This is useful in cases where + * you have a block that has parameters "MIN", "MAX", + * such as BlockLimit and you do not want an extra name + * to qualify them since the parent block has no "MIN", + * "MAX" parameters. + * + * Block Parameter Construction + * + * Block parameters are named constants, they are + * constructed using: + * BlockParam::BlockParam(Block * parent, const char * name) + * This funciton takes both a parent block and a name. + * The constructore then uses the parent name and the name of + * the paramter to ask the px4 param library if it has any + * parameters with this name. If it does, a handle to the + * parameter is retrieved. + * + * Block/ BlockParam Naming + * + * When desigining new blocks, the naming of the parameters and + * blocks determines the fully qualified name of the parameters + * within the ground station, so it is important to choose + * short, easily understandable names. Again, when a name of + * "" is passed, the parent block name is used as the value to + * prepend to paramters names. + */ + BlockYawDamper(SuperBlock *parent, const char *name); + /** + * Block deconstructor + * + * It is always a good idea to declare a virtual + * deconstructor so that upon calling delete from + * a class derived from this, all of the + * deconstructors all called, the derived class first, and + * then the base class + */ + virtual ~BlockYawDamper(); + + /** + * Block update function + * + * The job of the update function is to compute the output + * values for the block. In a simple block with one output, + * the output may be returned directly. If the output is + * required frequenly by other processses, it might be a + * good idea to declare a member to store the temporary + * variable. + */ + void update(float rCmd, float r); + + /** + * Rudder output value accessor + * + * This is a public accessor function, which means that the + * private value _rudder is returned to anyone calling + * BlockYawDamper::getRudder(). Note thate a setRudder() is + * not provided, this is because the updateParams() call + * for a block provides the mechanism for updating the + * paramter. + */ + float getRudder() { return _rudder; } +}; + +/** + * Stability augmentation system. + * Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299 + */ +class __EXPORT BlockStabilization : public SuperBlock +{ +private: + BlockYawDamper _yawDamper; + BlockLowPass _pLowPass; + BlockLowPass _qLowPass; + BlockP _p2Ail; + BlockP _q2Elv; + float _aileron; + float _elevator; +public: + BlockStabilization(SuperBlock *parent, const char *name); + virtual ~BlockStabilization(); + void update(float pCmd, float qCmd, float rCmd, + float p, float q, float r); + float getAileron() { return _aileron; } + float getElevator() { return _elevator; } + float getRudder() { return _yawDamper.getRudder(); } +}; + +/** + * Heading hold autopilot block. + * Aircraft Control and Simulation, Stevens and Lewis + * Heading hold, pg. 348 + */ +class __EXPORT BlockHeadingHold : public SuperBlock +{ +private: + BlockP _psi2Phi; + BlockP _phi2P; + BlockLimitSym _phiLimit; +public: + BlockHeadingHold(SuperBlock *parent, const char *name); + virtual ~BlockHeadingHold(); + /** + * returns pCmd + */ + float update(float psiCmd, float phi, float psi, float p); +}; + +/** + * Frontside/ Backside Control Systems + * + * Frontside : + * velocity error -> throttle + * altitude error -> elevator + * + * Backside : + * velocity error -> elevator + * altitude error -> throttle + * + * Backside control systems are more resilient at + * slow speeds on the back-side of the power + * required curve/ landing etc. Less performance + * than frontside at high speeds. + */ + +/** + * Backside velocity hold autopilot block. + * v -> theta -> q -> elevator + */ +class __EXPORT BlockVelocityHoldBackside : public SuperBlock +{ +private: + BlockPID _v2Theta; + BlockPID _theta2Q; + BlockLimit _theLimit; + BlockLimit _vLimit; +public: + BlockVelocityHoldBackside(SuperBlock *parent, const char *name); + virtual ~BlockVelocityHoldBackside(); + /** + * returns qCmd + */ + float update(float vCmd, float v, float theta, float q); +}; + +/** + * Frontside velocity hold autopilot block. + * v -> throttle + */ +class __EXPORT BlockVelocityHoldFrontside : public SuperBlock +{ +private: + BlockPID _v2Thr; +public: + BlockVelocityHoldFrontside(SuperBlock *parent, const char *name); + virtual ~BlockVelocityHoldFrontside(); + /** + * returns throttle + */ + float update(float vCmd, float v); +}; + +/** + * Backside altitude hold autopilot block. + * h -> throttle + */ +class __EXPORT BlockAltitudeHoldBackside : public SuperBlock +{ +private: + BlockPID _h2Thr; + float _throttle; +public: + BlockAltitudeHoldBackside(SuperBlock *parent, const char *name); + virtual ~BlockAltitudeHoldBackside(); + void update(float hCmd, float h); + float getThrottle() { return _throttle; } +}; + +/** + * Frontside altitude hold autopilot block. + * h -> theta > q -> elevator + */ +class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock +{ +private: + BlockPID _h2Theta; + BlockPID _theta2Q; +public: + BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name); + virtual ~BlockAltitudeHoldFrontside(); + /** + * return qCmd + */ + float update(float hCmd, float h, float theta, float q); +}; + +/** + * Backside autopilot + */ +class __EXPORT BlockBacksideAutopilot : public SuperBlock +{ +private: + BlockStabilization *_stabilization; + BlockHeadingHold _headingHold; + BlockVelocityHoldBackside _velocityHold; + BlockAltitudeHoldBackside _altitudeHold; + BlockParam _trimAil; + BlockParam _trimElv; + BlockParam _trimRdr; + BlockParam _trimThr; +public: + BlockBacksideAutopilot(SuperBlock *parent, + const char *name, + BlockStabilization *stabilization); + virtual ~BlockBacksideAutopilot(); + void update(float hCmd, float vCmd, float rCmd, float psiCmd, + float h, float v, + float phi, float theta, float psi, + float p, float q, float r); + float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); } + float getAileron() { return _stabilization->getAileron() + _trimAil.get(); } + float getElevator() { return _stabilization->getElevator() + _trimElv.get(); } + float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); } +}; + +/** + * 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(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd); + float getPsiCmd() { return _psiCmd; } +}; + +/** + * UorbEnabledAutopilot + */ +class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock +{ +protected: + // subscriptions + UOrbSubscription _att; + UOrbSubscription _attCmd; + UOrbSubscription _ratesCmd; + UOrbSubscription _pos; + UOrbSubscription _posCmd; + UOrbSubscription _manual; + UOrbSubscription _status; + UOrbSubscription _param_update; + // publications + UOrbPublication _actuators; +public: + BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); + virtual ~BlockUorbEnabledAutopilot(); +}; + +/** + * Multi-mode Autopilot + */ +class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot +{ +private: + BlockStabilization _stabilization; + BlockBacksideAutopilot _backsideAutopilot; + BlockWaypointGuidance _guide; + BlockParam _vCmd; + + struct pollfd _attPoll; + vehicle_global_position_setpoint_s _lastPosCmd; + enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; + uint64_t _timeStamp; +public: + BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name); + void update(); + virtual ~BlockMultiModeBacksideAutopilot(); +}; + + +} // namespace fixedwing + +} // namespace control + diff --git a/apps/systemlib/control/test_params.c b/apps/systemlib/control/test_params.c new file mode 100644 index 000000000..7c609cad3 --- /dev/null +++ b/apps/systemlib/control/test_params.c @@ -0,0 +1,22 @@ +#include +// WARNING: +// do not changes these unless +// you want to recompute the +// answers for all of the unit tests + +PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f); +PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f); +PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f); +PARAM_DEFINE_FLOAT(TEST_HP, 10.0f); +PARAM_DEFINE_FLOAT(TEST_LP, 10.0f); + +PARAM_DEFINE_FLOAT(TEST_P, 0.2f); + +PARAM_DEFINE_FLOAT(TEST_I, 0.1f); +PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f); + +PARAM_DEFINE_FLOAT(TEST_D, 0.01f); +PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f); + +PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f); +PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f); -- cgit v1.2.3