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/controllib/block | |
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/controllib/block')
-rw-r--r-- | apps/controllib/block/Block.cpp | 210 | ||||
-rw-r--r-- | apps/controllib/block/Block.hpp | 131 | ||||
-rw-r--r-- | apps/controllib/block/BlockParam.cpp | 77 | ||||
-rw-r--r-- | apps/controllib/block/BlockParam.hpp | 85 | ||||
-rw-r--r-- | apps/controllib/block/List.hpp | 71 | ||||
-rw-r--r-- | apps/controllib/block/UOrbPublication.cpp | 39 | ||||
-rw-r--r-- | apps/controllib/block/UOrbPublication.hpp | 118 | ||||
-rw-r--r-- | apps/controllib/block/UOrbSubscription.cpp | 51 | ||||
-rw-r--r-- | apps/controllib/block/UOrbSubscription.hpp | 137 |
9 files changed, 919 insertions, 0 deletions
diff --git a/apps/controllib/block/Block.cpp b/apps/controllib/block/Block.cpp new file mode 100644 index 000000000..5994d2315 --- /dev/null +++ b/apps/controllib/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 <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/controllib/block/Block.hpp b/apps/controllib/block/Block.hpp new file mode 100644 index 000000000..258701f27 --- /dev/null +++ b/apps/controllib/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 <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/controllib/block/BlockParam.cpp b/apps/controllib/block/BlockParam.cpp new file mode 100644 index 000000000..b3f49f7db --- /dev/null +++ b/apps/controllib/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 <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/controllib/block/BlockParam.hpp b/apps/controllib/block/BlockParam.hpp new file mode 100644 index 000000000..7f86d1717 --- /dev/null +++ b/apps/controllib/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 <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/controllib/block/List.hpp b/apps/controllib/block/List.hpp new file mode 100644 index 000000000..96b0b94d1 --- /dev/null +++ b/apps/controllib/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 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/controllib/block/UOrbPublication.cpp b/apps/controllib/block/UOrbPublication.cpp new file mode 100644 index 000000000..f69b39d90 --- /dev/null +++ b/apps/controllib/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/controllib/block/UOrbPublication.hpp b/apps/controllib/block/UOrbPublication.hpp new file mode 100644 index 000000000..a36f4429f --- /dev/null +++ b/apps/controllib/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 <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/controllib/block/UOrbSubscription.cpp b/apps/controllib/block/UOrbSubscription.cpp new file mode 100644 index 000000000..022cadd24 --- /dev/null +++ b/apps/controllib/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/controllib/block/UOrbSubscription.hpp b/apps/controllib/block/UOrbSubscription.hpp new file mode 100644 index 000000000..22cc2e114 --- /dev/null +++ b/apps/controllib/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 <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 |