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

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

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

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);

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 <inttypes.h>
+
 class AP_VarI
 {
 public:
@@ -148,4 +151,7 @@ typedef AP_Var<int32_t> AP_Int32;
 typedef AP_Var<uint32_t> AP_Unt32;
 typedef AP_Var<bool> AP_Bool;
 
+extern AP_Int8 AP_unity;
+extern AP_Int8 AP_negativeUnity;
+
 #endif // AP_Var_H

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)
 {

libraries/AP_Controller/AP_Controller.cpp

@@ -0,0 +1,21 @@
+/*
+ * AP_Controller.cpp
+ * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_Controller.h"
+
+// vim:ts=4:sw=4:expandtab

libraries/AP_Controller/AP_Controller.h

@@ -0,0 +1,202 @@
+/*
+ * AP_Controller.h
+ * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef AP_Controller_H
+#define AP_Controller_H
+
+#include <AP_Vector.h>
+#include <AP_Var.h>
+#include <AP_RcChannel.h>
+#include <APM_RC.h>
+
+/// 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<Block * > _blocks;	
+    Vector<AP_RcChannel * > _rc;
+};
+
+#endif // AP_Controller_H
+
+// vim:ts=4:sw=4:expandtab

libraries/AP_Controller/examples/Car/Car.pde

@@ -0,0 +1,63 @@
+#include <FastSerial.h>
+#include <AP_Controller.h>
+#include <AP_Var.h>
+#include <AP_EEProm.h>
+#include <AP_RcChannel.h>
+#include <APM_RC.h>
+
+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();
+}

libraries/AP_RcChannel/AP_RcChannel.h

@@ -7,7 +7,6 @@
 #define AP_RcChannel_h
 
 #include <stdint.h>
-#include <FastSerial.h>
 #include <APM_RC.h>
 #include <AP_Var.h>
 #include <AP_Common.h>
@@ -84,7 +83,3 @@ private:
 };
 
 #endif	
-
-
-
-