forked from Archive/PX4-Autopilot
Merge commit '8888b73e160520e5b15e168998013f4a5f6e64c0' into local/mathlib
This commit is contained in:
commit
950d104c8d
|
@ -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
|
|
@ -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();
|
||||||
|
}
|
|
@ -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);
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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;
|
||||||
|
};
|
|
@ -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"
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
Loading…
Reference in New Issue