From 634f2734b50f33dcd96c1f4e2a5dfa649f5eff3c Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Fri, 31 Dec 2010 06:20:28 +0000 Subject: [PATCH] Added AP_Controller library. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1379 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Common/AP_Loop.cpp | 5 - libraries/AP_Common/AP_Loop.h | 9 - libraries/AP_Common/AP_Var.cpp | 15 ++ libraries/AP_Common/AP_Var.h | 6 + libraries/AP_Common/c++.cpp | 10 +- libraries/AP_Controller/AP_Controller.cpp | 21 ++ libraries/AP_Controller/AP_Controller.h | 202 +++++++++++++++++++ libraries/AP_Controller/examples/Car/Car.pde | 63 ++++++ libraries/AP_RcChannel/AP_RcChannel.h | 5 - 9 files changed, 312 insertions(+), 24 deletions(-) create mode 100644 libraries/AP_Common/AP_Var.cpp create mode 100644 libraries/AP_Controller/AP_Controller.cpp create mode 100644 libraries/AP_Controller/AP_Controller.h create mode 100644 libraries/AP_Controller/examples/Car/Car.pde diff --git a/libraries/AP_Common/AP_Loop.cpp b/libraries/AP_Common/AP_Loop.cpp index 1146802b15..82040d72ee 100644 --- a/libraries/AP_Common/AP_Loop.cpp +++ b/libraries/AP_Common/AP_Loop.cpp @@ -18,9 +18,6 @@ #include "AP_Loop.h" -namespace apo -{ - Loop::Loop(float frequency, void (*fptr)(void *), void * data) : _fptr(fptr), _data(data), @@ -52,6 +49,4 @@ void Loop::update() _load = 0.9*_load + 10*(float(micros()-_timeStamp)/(_timeStamp-timeStamp0)); } -} // apo - // vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Common/AP_Loop.h b/libraries/AP_Common/AP_Loop.h index b4c6792851..c41935c8aa 100644 --- a/libraries/AP_Common/AP_Loop.h +++ b/libraries/AP_Common/AP_Loop.h @@ -21,13 +21,6 @@ #include "AP_Vector.h" -/// -// Start of apo namespace -// The above functions must be in the global -// namespace for c++ to function properly -namespace apo -{ - class Loop { public: @@ -62,8 +55,6 @@ private: float _dt; }; -} // apo - #endif // vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Common/AP_Var.cpp b/libraries/AP_Common/AP_Var.cpp new file mode 100644 index 0000000000..4fc41e079c --- /dev/null +++ b/libraries/AP_Common/AP_Var.cpp @@ -0,0 +1,15 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// This is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License as published by the +// Free Software Foundation; either version 2.1 of the License, or (at +// your option) any later version. +// +/// The AP variable interface. This allows different types +/// of variables to be passed to blocks for floating point +/// math, memory management, etc. + +#include "AP_Var.h" + +extern AP_Int8 AP_unity(1); +extern AP_Int8 AP_negativeUnity(-1); diff --git a/libraries/AP_Common/AP_Var.h b/libraries/AP_Common/AP_Var.h index 6ef976aca8..dee5451816 100644 --- a/libraries/AP_Common/AP_Var.h +++ b/libraries/AP_Common/AP_Var.h @@ -8,9 +8,12 @@ /// The AP variable interface. This allows different types /// of variables to be passed to blocks for floating point /// math, memory management, etc. + #ifndef AP_Var_H #define AP_Var_H +#include + class AP_VarI { public: @@ -148,4 +151,7 @@ typedef AP_Var AP_Int32; typedef AP_Var AP_Unt32; typedef AP_Var AP_Bool; +extern AP_Int8 AP_unity; +extern AP_Int8 AP_negativeUnity; + #endif // AP_Var_H diff --git a/libraries/AP_Common/c++.cpp b/libraries/AP_Common/c++.cpp index d0f54b940f..c1307ea13f 100644 --- a/libraries/AP_Common/c++.cpp +++ b/libraries/AP_Common/c++.cpp @@ -24,14 +24,14 @@ void operator delete(void *p) if (p) free(p); } -#if 0 -// We should never need this, as classes should never be defined -// with pure virtual member functions extern "C" void __cxa_pure_virtual() { - while (1); + while (1) + { + Serial.println("Error: pure virtual call"); + delay(1000); + } } -#endif void * operator new[](size_t size) { diff --git a/libraries/AP_Controller/AP_Controller.cpp b/libraries/AP_Controller/AP_Controller.cpp new file mode 100644 index 0000000000..078a94e2e9 --- /dev/null +++ b/libraries/AP_Controller/AP_Controller.cpp @@ -0,0 +1,21 @@ +/* + * AP_Controller.cpp + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "AP_Controller.h" + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Controller/AP_Controller.h b/libraries/AP_Controller/AP_Controller.h new file mode 100644 index 0000000000..1453d3e8ce --- /dev/null +++ b/libraries/AP_Controller/AP_Controller.h @@ -0,0 +1,202 @@ +/* + * AP_Controller.h + * Copyright (C) James Goppert 2010 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#ifndef AP_Controller_H +#define AP_Controller_H + +#include +#include +#include +#include + +/// Block +class Block +{ +public: + Block() : + _input(), _output() + { + } + virtual void update(const float & dt = 0) = 0; + virtual void connect( Block * block) + { + } + const char * getName() { return _name; } + const Vector < AP_VarI * > & getOutput() const { return _output; } +protected: + const char * _name; + Vector< AP_VarI * > _input; + Vector< AP_VarI * > _output; +}; + +/// Servo Block +class ToServo: public Block +{ +public: + ToServo(AP_RcChannel * ch) : _ch(ch) + { + } + virtual void connect(Block * block) + { + if (block->getOutput().getSize() > 0) + _input.push_back(block->getOutput()[0]); + } + virtual void update(const float & dt = 0) + { + if (_input.getSize() > 0) + _ch->setPosition(_output[0]->getF()); + } +private: + float _position; + AP_RcChannel * _ch; +}; + +/// SumGain +class SumGain : public Block +{ +public: + /// Constructor that allows 1-8 sum gain pairs, more + /// can be added if necessary + SumGain( + AP_VarI * var1, AP_VarI * gain1, + AP_VarI * var2 = NULL, AP_VarI * gain2 = NULL, + AP_VarI * var3 = NULL, AP_VarI * gain3 = NULL, + AP_VarI * var4 = NULL, AP_VarI * gain4 = NULL, + AP_VarI * var5 = NULL, AP_VarI * gain5 = NULL, + AP_VarI * var6 = NULL, AP_VarI * gain6 = NULL, + AP_VarI * var7 = NULL, AP_VarI * gain7 = NULL, + AP_VarI * var8 = NULL, AP_VarI * gain8 = NULL) : + _gain() + { + _output.push_back(new AP_Float(0,"","")); + if (var1 && gain1) add(var1,gain1); + if (var2 && gain2) add(var2,gain2); + if (var3 && gain3) add(var3,gain3); + if (var4 && gain4) add(var4,gain4); + if (var5 && gain5) add(var5,gain5); + if (var6 && gain6) add(var6,gain6); + if (var7 && gain7) add(var7,gain7); + if (var8 && gain8) add(var8,gain8); + } + void add(AP_VarI * var, AP_VarI * gain) + { + _input.push_back(var); + _gain.push_back(gain); + } + virtual void connect(Block * block) + { + if (block->getOutput().getSize() > 0) + _input.push_back(block->getOutput()[0]); + } + virtual void update(const float & dt = 0) + { + if (_output.getSize() < 1) return; + _output[0]->setF(0); + for (int i=0;i<_input.getSize();i++) + { + _output[0]->setF( _output[i]->getF() + _input[i]->getF()*_gain[i]->getF()); + } + } +private: + Vector< AP_VarI * > _gain; +}; + +/// PID block +class Pid : public Block +{ +public: + Pid(const char * name="", + const float & kP=0, + const float & kI=0, + const float & kD=0, + const float & iMax=1, + const uint8_t & dFcut=20 + ) : + _kP(new AP_EEPROM_Float(kP,"KP",name)), + _kI(new AP_EEPROM_Float(kI,"KI",name)), + _kD(new AP_EEPROM_Float(kD,"KD",name)), + _iMax(new AP_EEPROM_Float(iMax,"IMAX",name)), + _dFcut(new AP_EEPROM_Uint8(dFcut,"DFCUT",name)) + { + _output.push_back(new AP_Float(0,"OUT",name)); + } + virtual void connect(Block * block) + { + if (block->getOutput().getSize() > 0) + _input.push_back(block->getOutput()[0]); + } + virtual void update(const float & dt = 0) + { + if (_output.getSize() < 1) return; + + // derivative + float RC = 1/(2*M_PI*_dFcut->get()); // low pass filter + _eD = _eD + ( ((_e - _input[0]->getF()))/dt - _eD ) * (dt / (dt + RC)); + + // proportional, note must come after derivative + // because derivatve uses _e as previous value + _e = _input[0]->getF(); + + // integral + _eI += _e*dt; + + // pid sum + _output[0]->setF(_kP->getF()*_e + _kI->getF()*_eI + _kD->getF()*_eD); + } +private: + float _e; /// input + float _eI; /// integral of input + float _eD; /// derivative of input + AP_Float * _kP; /// proportional gain + AP_Float * _kI; /// integral gain + AP_Float * _kD; /// derivative gain + AP_Float * _iMax; /// integrator saturation + AP_Uint8 * _dFcut; /// derivative low-pass cut freq (Hz) +}; + +/// Controller class +class AP_Controller +{ +public: + void addBlock(Block * block) + { + if (_blocks.getSize() > 0) + _blocks[_blocks.getSize()]->connect(block); + _blocks.push_back(block); + } + void addCh(AP_RcChannel * ch) + { + _rc.push_back(ch); + } + AP_RcChannel * getRc(int i) + { + return _rc[i]; + } + void update() + { + for (int i=0;i<_blocks.getSize();i++) + _blocks[i]->update(); + } +private: + Vector _blocks; + Vector _rc; +}; + +#endif // AP_Controller_H + +// vim:ts=4:sw=4:expandtab diff --git a/libraries/AP_Controller/examples/Car/Car.pde b/libraries/AP_Controller/examples/Car/Car.pde new file mode 100644 index 0000000000..dff4bb2749 --- /dev/null +++ b/libraries/AP_Controller/examples/Car/Car.pde @@ -0,0 +1,63 @@ +#include +#include +#include +#include +#include +#include + +FastSerialPort0(Serial); + +class CarController : public AP_Controller +{ +private: + // state + AP_Float * roll; + AP_Float * airspeed; + AP_Float * velocity; + AP_Float * heading; + + // servo positions + AP_Float * steering; + AP_Float * throttle; + + // control variables + AP_Float * headingCommand; + AP_Float * velocityCommand; + + // channels + uint8_t chStr; + uint8_t chThr; +public: + CarController() : + chStr(0), chThr(1) + { + // rc channels + addCh(new AP_RcChannel("STR",APM_RC,chStr,45)); + addCh(new AP_RcChannel("THR",APM_RC,chThr,100)); + + // steering control loop + addBlock(new SumGain(headingCommand,&AP_unity,heading,&AP_negativeUnity)); + addBlock(new Pid("HDNG",0.1,0,0,1,20)); + addBlock(new ToServo(getRc(chStr))); + + // throttle control loop + addBlock(new SumGain(velocityCommand,&AP_unity,velocity,&AP_negativeUnity)); + addBlock(new Pid("THR",0.1,0,0,1,20)); + addBlock(new ToServo(getRc(chThr))); + } +}; + +AP_Controller * controller; + +void setup() +{ + Serial.begin(115200); + //controller = new CarController; +} + +void loop() +{ + Serial.println("Looping"); + delay(1000); + //controller->update(); +} diff --git a/libraries/AP_RcChannel/AP_RcChannel.h b/libraries/AP_RcChannel/AP_RcChannel.h index faac003159..9a1ec1bee1 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.h +++ b/libraries/AP_RcChannel/AP_RcChannel.h @@ -7,7 +7,6 @@ #define AP_RcChannel_h #include -#include #include #include #include @@ -84,7 +83,3 @@ private: }; #endif - - - -