dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge commit '8888b73e160520e5b15e168998013f4a5f6e64c0' into local/mathlib

This commit is contained in:
px4dev 2013-01-06 13:58:52 -08:00
parent 43ccb257a8 8888b73e16
commit 950d104c8d
17 changed files with 2983 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
apps/examples/control_demo/Makefile Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# Basic example application
#
APPNAME = control_demo
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk

168
apps/examples/control_demo/control_demo.cpp Normal file
View File

@ -0,0 +1,168 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* 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 control_demo.cpp
* Demonstration of control library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/control/fixedwing.hpp>
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
#include <math.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int control_demo_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int control_demo_thread_main(int argc, char *argv[]);
/**
* Test function
*/
void test();
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int control_demo_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("control_demo already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("control_demo",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
4096,
control_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "test")) {
test();
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tcontrol_demo app is running\n");
} else {
printf("\tcontrol_demo app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int control_demo_thread_main(int argc, char *argv[])
{
printf("[control_Demo] starting\n");
using namespace control;
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
thread_running = true;
while (!thread_should_exit) {
autopilot.update();
}
printf("[control_demo] exiting.\n");
thread_running = false;
return 0;
}
void test()
{
printf("beginning control lib test\n");
control::basicBlocksTest();
}

63
apps/examples/control_demo/params.c Normal file
View File

@ -0,0 +1,63 @@
#include <systemlib/param/param.h>
// currently tuned for easystar from arkhangar in HIL
//https://github.com/arktools/arkhangar
// 16 is max name length
// gyro low pass filter
PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq
PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq
PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq
// yaw washout
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
// stabilization mode
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.1f); // roll rate 2 aileron
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
// psi -> phi -> p
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f); // heading 2 roll
PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f); // roll to roll rate
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f); // roll limit
// velocity -> theta
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f);
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f);
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f);
// theta -> q
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
// h -> thr
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.005f);
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f);
// crosstrack
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.0f);
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f);
// speed command
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
// trim
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f);
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f);

210
apps/systemlib/control/block/Block.cpp Normal file
View File

@ -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

131
apps/systemlib/control/block/Block.hpp Normal file
View File

@ -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

77
apps/systemlib/control/block/BlockParam.cpp Normal file
View File

@ -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

85
apps/systemlib/control/block/BlockParam.hpp Normal file
View File

@ -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

71
apps/systemlib/control/block/List.hpp Normal file
View File

@ -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;
};

39
apps/systemlib/control/block/UOrbPublication.cpp Normal file
View File

@ -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"

118
apps/systemlib/control/block/UOrbPublication.hpp Normal file
View File

@ -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

51
apps/systemlib/control/block/UOrbSubscription.cpp Normal file
View File

@ -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

137
apps/systemlib/control/block/UOrbSubscription.hpp Normal file
View File

@ -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

486
apps/systemlib/control/blocks.cpp Normal file
View File

@ -0,0 +1,486 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file blocks.cpp
*
* Controller library code
*/
#include <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

494
apps/systemlib/control/blocks.hpp Normal file
View File

@ -0,0 +1,494 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file blocks.h
*
* Controller library code
*/
#pragma once
#include <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

351
apps/systemlib/control/fixedwing.cpp Normal file
View File

@ -0,0 +1,351 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fixedwing.cpp
*
* Controller library code
*/
#include "fixedwing.hpp"
namespace control
{
namespace fixedwing
{
BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_rLowPass(this, "R_LP"),
_rWashout(this, "R_HP"),
_r2Rdr(this, "R2RDR"),
_rudder(0)
{
}
BlockYawDamper::~BlockYawDamper() {};
void BlockYawDamper::update(float rCmd, float r)
{
_rudder = _r2Rdr.update(rCmd -
_rWashout.update(_rLowPass.update(r)));
}
BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_yawDamper(this, ""),
_pLowPass(this, "P_LP"),
_qLowPass(this, "Q_LP"),
_p2Ail(this, "P2AIL"),
_q2Elv(this, "Q2ELV"),
_aileron(0),
_elevator(0)
{
}
BlockStabilization::~BlockStabilization() {};
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
float p, float q, float r)
{
_aileron = _p2Ail.update(
pCmd - _pLowPass.update(p));
_elevator = _q2Elv.update(
qCmd - _qLowPass.update(q));
_yawDamper.update(rCmd, r);
}
BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_psi2Phi(this, "PSI2PHI"),
_phi2P(this, "PHI2P"),
_phiLimit(this, "PHI_LIM")
{
}
BlockHeadingHold::~BlockHeadingHold() {};
float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
{
float psiError = _wrap_pi(psiCmd - psi);
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
return _phi2P.update(phiCmd - phi);
}
BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_v2Theta(this, "V2THE"),
_theta2Q(this, "THE2Q"),
_theLimit(this, "THE"),
_vLimit(this, "V")
{
}
BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
{
// negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
return _theta2Q.update(thetaCmd - theta);
}
BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_v2Thr(this, "V2THR")
{
}
BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
float BlockVelocityHoldFrontside::update(float vCmd, float v)
{
return _v2Thr.update(vCmd - v);
}
BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_h2Thr(this, "H2THR"),
_throttle(0)
{
}
BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
void BlockAltitudeHoldBackside::update(float hCmd, float h)
{
_throttle = _h2Thr.update(hCmd - h);
}
BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_h2Theta(this, "H2THE"),
_theta2Q(this, "THE2Q")
{
}
BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
{
float thetaCmd = _h2Theta.update(hCmd - h);
return _theta2Q.update(thetaCmd - theta);
}
BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
const char *name,
BlockStabilization *stabilization) :
SuperBlock(parent, name),
_stabilization(stabilization),
_headingHold(this, ""),
_velocityHold(this, ""),
_altitudeHold(this, ""),
_trimAil(this, "TRIM_AIL"),
_trimElv(this, "TRIM_ELV"),
_trimRdr(this, "TRIM_RDR"),
_trimThr(this, "TRIM_THR")
{
}
BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
float h, float v,
float phi, float theta, float psi,
float p, float q, float r)
{
_altitudeHold.update(hCmd, h);
_stabilization->update(
_headingHold.update(psiCmd, phi, psi, p),
_velocityHold.update(vCmd, v, theta, q),
rCmd,
p, q, r);
}
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_xtYawLimit(this, "XT2YAW"),
_xt2Yaw(this, "XT2YAW"),
_psiCmd(0)
{
}
BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd)
{
// heading to waypoint
float psiTrack = get_bearing_to_next_waypoint(
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
// cross track
struct crosstrack_error_s xtrackError;
get_distance_to_line(&xtrackError,
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)lastPosCmd.lat / (double)1e7d,
(double)lastPosCmd.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
_psiCmd = _wrap_2pi(psiTrack -
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
}
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_actuators(&getPublications(), ORB_ID(actuator_controls_0))
{
}
BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
BlockUorbEnabledAutopilot(parent, name),
_stabilization(this, ""), // no name needed, already unique
_backsideAutopilot(this, "", &_stabilization),
_guide(this, ""),
_vCmd(this, "V_CMD"),
_attPoll(),
_lastPosCmd(),
_timeStamp(0)
{
_attPoll.fd = _att.getHandle();
_attPoll.events = POLLIN;
}
void BlockMultiModeBacksideAutopilot::update()
{
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// store old position command before update if new command sent
if (_posCmd.updated()) {
_lastPosCmd = _posCmd.getData();
}
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_STABILIZED) {
_stabilization.update(
_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.state_machine == SYSTEM_STATE_AUTO) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// commands
float rCmd = 0;
_backsideAutopilot.update(
_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
_pos.alt, v,
_att.roll, _att.pitch, _att.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed
);
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
}
// update all publications
updatePublications();
}
BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{
// send one last publication when destroyed, setting
// all output to zero
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
updatePublications();
}
} // namespace fixedwing
} // namespace control

438
apps/systemlib/control/fixedwing.hpp Normal file
View File

@ -0,0 +1,438 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fixedwing.h
*
* Controller library code
*/
#pragma once
#include <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

22
apps/systemlib/control/test_params.c Normal file
View File

@ -0,0 +1,22 @@
#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);