diff options
author | px4dev <px4@purgatory.org> | 2013-01-06 15:04:30 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-01-06 15:04:30 -0800 |
commit | faced6bfe3826a4fdcfcd72171edbb501226814a (patch) | |
tree | 42714ce675153555255e30894408ece40f4d26c9 /apps/systemlib | |
parent | 950d104c8d7e335b88c0a7944628c14293a0f676 (diff) | |
download | px4-firmware-faced6bfe3826a4fdcfcd72171edbb501226814a.tar.gz px4-firmware-faced6bfe3826a4fdcfcd72171edbb501226814a.tar.bz2 px4-firmware-faced6bfe3826a4fdcfcd72171edbb501226814a.zip |
Merge James's controllib bits into a separate library module.
Add a top-level mathlib header to avoid having to dig around for specific headers.
Diffstat (limited to 'apps/systemlib')
-rw-r--r-- | apps/systemlib/control/block/Block.cpp | 210 | ||||
-rw-r--r-- | apps/systemlib/control/block/Block.hpp | 131 | ||||
-rw-r--r-- | apps/systemlib/control/block/BlockParam.cpp | 77 | ||||
-rw-r--r-- | apps/systemlib/control/block/BlockParam.hpp | 85 | ||||
-rw-r--r-- | apps/systemlib/control/block/List.hpp | 71 | ||||
-rw-r--r-- | apps/systemlib/control/block/UOrbPublication.cpp | 39 | ||||
-rw-r--r-- | apps/systemlib/control/block/UOrbPublication.hpp | 118 | ||||
-rw-r--r-- | apps/systemlib/control/block/UOrbSubscription.cpp | 51 | ||||
-rw-r--r-- | apps/systemlib/control/block/UOrbSubscription.hpp | 137 | ||||
-rw-r--r-- | apps/systemlib/control/blocks.cpp | 486 | ||||
-rw-r--r-- | apps/systemlib/control/blocks.hpp | 494 | ||||
-rw-r--r-- | apps/systemlib/control/fixedwing.cpp | 351 | ||||
-rw-r--r-- | apps/systemlib/control/fixedwing.hpp | 438 | ||||
-rw-r--r-- | apps/systemlib/control/test_params.c | 22 |
14 files changed, 0 insertions, 2710 deletions
diff --git a/apps/systemlib/control/block/Block.cpp b/apps/systemlib/control/block/Block.cpp deleted file mode 100644 index 5994d2315..000000000 --- a/apps/systemlib/control/block/Block.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Block.cpp - * - * Controller library code - */ - -#include <math.h> -#include <string.h> -#include <stdio.h> - -#include "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 deleted file mode 100644 index 258701f27..000000000 --- a/apps/systemlib/control/block/Block.hpp +++ /dev/null @@ -1,131 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Block.h - * - * Controller library code - */ - -#pragma once - -#include <stdint.h> -#include <inttypes.h> - -#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<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<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; } - List<UOrbPublicationBase *> & getPublications() { return _publications; } - List<BlockParamBase *> & getParams() { return _params; } -// attributes - const char *_name; - SuperBlock *_parent; - float _dt; - List<UOrbSubscriptionBase *> _subscriptions; - List<UOrbPublicationBase *> _publications; - List<BlockParamBase *> _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<Block *> & getChildren() { return _children; } - void updateChildParams(); - void updateChildSubscriptions(); - void updateChildPublications(); -// attributes - List<Block *> _children; -}; - -} // namespace control diff --git a/apps/systemlib/control/block/BlockParam.cpp b/apps/systemlib/control/block/BlockParam.cpp deleted file mode 100644 index b3f49f7db..000000000 --- a/apps/systemlib/control/block/BlockParam.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Blockparam.cpp - * - * Controller library code - */ - -#include <math.h> -#include <stdio.h> -#include <string.h> - -#include "BlockParam.hpp" - -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 deleted file mode 100644 index 7f86d1717..000000000 --- a/apps/systemlib/control/block/BlockParam.hpp +++ /dev/null @@ -1,85 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file BlockParam.h - * - * Controller library code - */ - -#pragma once - -#include <systemlib/param/param.h> - -#include "Block.hpp" -#include "List.hpp" - -namespace control -{ - -/** - * A base class for block params that enables traversing linked list. - */ -class __EXPORT BlockParamBase : public ListNode<BlockParamBase *> -{ -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 T> -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 deleted file mode 100644 index 96b0b94d1..000000000 --- a/apps/systemlib/control/block/List.hpp +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Node.h - * - * A node of a linked list. - */ - -#pragma once - -template<class T> -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 T> -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 deleted file mode 100644 index f69b39d90..000000000 --- a/apps/systemlib/control/block/UOrbPublication.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file UOrbPublication.cpp - * - */ - -#include "UOrbPublication.hpp" diff --git a/apps/systemlib/control/block/UOrbPublication.hpp b/apps/systemlib/control/block/UOrbPublication.hpp deleted file mode 100644 index a36f4429f..000000000 --- a/apps/systemlib/control/block/UOrbPublication.hpp +++ /dev/null @@ -1,118 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file UOrbPublication.h - * - */ - -#pragma once - -#include <uORB/uORB.h> -#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<control::UOrbPublicationBase *> -{ -public: - - UOrbPublicationBase( - List<UOrbPublicationBase *> * 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 T> -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<UOrbPublicationBase *> * 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 deleted file mode 100644 index 022cadd24..000000000 --- a/apps/systemlib/control/block/UOrbSubscription.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file 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 deleted file mode 100644 index 22cc2e114..000000000 --- a/apps/systemlib/control/block/UOrbSubscription.hpp +++ /dev/null @@ -1,137 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file UOrbSubscription.h - * - */ - -#pragma once - -#include <uORB/uORB.h> -#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<control::UOrbSubscriptionBase *> -{ -public: -// methods - - /** - * Constructor - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - */ - UOrbSubscriptionBase( - List<UOrbSubscriptionBase *> * 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 T> -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<UOrbSubscriptionBase *> * 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 deleted file mode 100644 index c6c374300..000000000 --- a/apps/systemlib/control/blocks.cpp +++ /dev/null @@ -1,486 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file blocks.cpp - * - * Controller library code - */ - -#include <math.h> -#include <stdio.h> - -#include "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 deleted file mode 100644 index 95f2021ce..000000000 --- a/apps/systemlib/control/blocks.hpp +++ /dev/null @@ -1,494 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file blocks.h - * - * Controller library code - */ - -#pragma once - -#include <assert.h> -#include <time.h> -#include <stdlib.h> -#include <systemlib/test/test.hpp> - -#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<float> _min; - BlockParam<float> _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<float> _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<float> _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<float> _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<float> _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<float> _kP; - BlockParam<float> _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<float> _kP; - BlockParam<float> _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<float> _kP; - BlockParam<float> _kI; - BlockParam<float> _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<float> _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<float> _min; - BlockParam<float> _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<float> _mean; - BlockParam<float> _stdDev; -}; - -int __EXPORT blockRandGaussTest(); - -} // namespace control diff --git a/apps/systemlib/control/fixedwing.cpp b/apps/systemlib/control/fixedwing.cpp deleted file mode 100644 index 695653714..000000000 --- a/apps/systemlib/control/fixedwing.cpp +++ /dev/null @@ -1,351 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file 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 deleted file mode 100644 index ea742641c..000000000 --- a/apps/systemlib/control/fixedwing.hpp +++ /dev/null @@ -1,438 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fixedwing.h - * - * Controller 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/vehicle_global_position_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/actuator_controls.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 "block/UOrbSubscription.hpp" -#include "block/UOrbPublication.hpp" - -extern "C" { -#include <systemlib/geo/geo.h> -} - -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<float> _trimAil; - BlockParam<float> _trimElv; - BlockParam<float> _trimRdr; - BlockParam<float> _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<vehicle_attitude_s> _att; - UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; - UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; - UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd; - UOrbSubscription<manual_control_setpoint_s> _manual; - UOrbSubscription<vehicle_status_s> _status; - UOrbSubscription<parameter_update_s> _param_update; - // publications - UOrbPublication<actuator_controls_s> _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<float> _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 deleted file mode 100644 index 7c609cad3..000000000 --- a/apps/systemlib/control/test_params.c +++ /dev/null @@ -1,22 +0,0 @@ -#include <systemlib/param/param.h> -// WARNING: -// do not changes these unless -// you want to recompute the -// answers for all of the unit tests - -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); |