Added Loop class.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1251 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_Common/AP_Common.h

@@ -18,6 +18,8 @@
 #include <stdint.h>
 #include "include/menu.h"		/// simple menu subsystem
 #include "c++.h" // c++ additions
+#include "AP_Vector.h"
+#include "AP_Loop.h"
 
 ////////////////////////////////////////////////////////////////////////////////
 /// @name	Warning control

libraries/AP_Common/AP_Loop.cpp

@@ -0,0 +1,57 @@
+/*
+ * AP_Loop.pde
+ * 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_Loop.h"
+
+namespace apo
+{
+
+Loop::Loop(float frequency, void (*fptr)(void *), void * data) :
+    _fptr(fptr),
+    _data(data),
+    _period(1.0e6/frequency),
+    _timeStamp(micros()),
+    _subLoops(),
+    _load(0),
+    _dt(0)
+{
+}
+
+void Loop::update()
+{
+    // quick exit if not ready
+    if (micros() - _timeStamp < _period) return;
+
+    // update time stamp
+    uint32_t timeStamp0 = _timeStamp;
+    _timeStamp = micros();
+    _dt = (_timeStamp - timeStamp0)/1.0e6;
+
+    // update sub loops
+    for (int i=0; i<_subLoops.getSize(); i++) _subLoops[i]->update();
+
+    // callback function
+    if (_fptr) _fptr(_data);
+
+    // calculated load with a low pass filter
+    _load = 0.9*_load + 10*(float(micros()-_timeStamp)/(_timeStamp-timeStamp0));
+}
+
+} // apo
+
+// vim:ts=4:sw=4:expandtab

libraries/AP_Common/AP_Loop.h

@@ -0,0 +1,70 @@
+/*
+ * AP_Loop.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_Loop_H
+#define AP_Loop_H
+
+#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:
+    Loop() : _fptr(), _data(), _period(), _subLoops(), _timeStamp(), _load(), _dt() {};
+    Loop(float frequency, void (*fptr)(void *) = NULL, void * data = NULL);
+    void update();
+    Vector<Loop *> & subLoops() {
+        return _subLoops;
+    }
+    uint32_t frequency() {
+        return 1.0e6/_period;
+    }
+    void frequency(float frequency) {
+        _period = 1e6/frequency;
+    }
+    uint32_t timeStamp() {
+        return _timeStamp;
+    }
+    float dt() {
+        return _dt;
+    }
+    uint8_t load() {
+        return _load;
+    }
+private:
+    void (*_fptr)(void *);
+    void * _data;
+    uint32_t _period;
+    Vector<Loop *> _subLoops;
+    uint32_t _timeStamp;
+    uint8_t _load;
+    float _dt;
+};
+
+} // apo
+
+#endif
+
+// vim:ts=4:sw=4:expandtab

libraries/AP_Common/AP_Vector.h

@@ -1,5 +1,5 @@
 /*
- * AP_Vector.h
+ * Vector.h
  * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
  *
  * This file is free software: you can redistribute it and/or modify it
@@ -16,20 +16,20 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef AP_Vector_H
-#define AP_Vector_H
+#ifndef Vector_H
+#define Vector_H
 
 #include <stdlib.h>
 #include <inttypes.h>
 #include <WProgram.h>
 
 #ifdef ASSERT
-const static char vectorSource[] ="AP_Vector.hpp";
+const static char vectorSource[] ="Vector.hpp";
 #endif
 
 // vector
 template <class dataType,class sumType=dataType>
-class AP_Vector
+class Vector
 {
 private:
     size_t size;
@@ -38,38 +38,38 @@ private:
     dataType* data;
 public:
     // default constructor
-    AP_Vector(const size_t & size=0, const size_t & extraAllocationSize=0) : size(0), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {
+    Vector(const size_t & size=0, const size_t & extraAllocationSize=0) : size(0), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {
         setSize(size);
     }
     // 3 vector constructor
-    AP_Vector(const dataType & a, const dataType & b, const dataType & c) : size(3), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {
+    Vector(const dataType & a, const dataType & b, const dataType & c) : size(3), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {
         setSize(size);
-        (*this)(0)=a;
-        (*this)(1)=b;
-        (*this)(2)=c;
+        (*this)[0]=a;
+        (*this)[1]=b;
+        (*this)[2]=c;
     }
 
     // construct from array
-    AP_Vector(const dataType* array, const size_t & size, const size_t & extraAllocationSize=0) : size(0), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {
+    Vector(const dataType* array, const size_t & size, const size_t & extraAllocationSize=0) : size(0), extraAllocationSize(extraAllocationSize), sizeAllocated(0), data(NULL) {
         setSize(size);
         for (size_t i=0; i<getSize(); i++)
-            (*this)(i)=array[i];
+            (*this)[i]=array[i];
     }
     // copy constructor
-    AP_Vector(const AP_Vector &v) : size(0), extraAllocationSize(0), sizeAllocated(0), data(NULL) {
+    Vector(const Vector &v) : size(0), extraAllocationSize(0), sizeAllocated(0), data(NULL) {
         setSize(v.getSize());
         for (size_t i=0; i<getSize(); i++)
-            (*this)(i)=v(i);
+            (*this)[i]=v[i];
     }
     // convert to float vector
-    AP_Vector<float> toFloat() const {
-        AP_Vector<float> v(getSize());
+    Vector<float> toFloat() const {
+        Vector<float> v(getSize());
         for (size_t i=0; i<getSize(); i++)
-            v(i)=(*this)(i);
+            v[i]=(*this)[i];
         return v;
     }
     // destructor
-    virtual ~AP_Vector() {
+    virtual ~Vector() {
         empty();
     }
     void empty() {
@@ -116,7 +116,7 @@ public:
             memmove(data+index+1,data+index,sizeof(dataType)/sizeof(char)*(getSize()-1-index));
             //Serial.println("memmove called and completed");
         }
-        (*this)(index)=value;
+        (*this)[index]=value;
         //Serial.println("insert done");
     }
     // remove
@@ -135,149 +135,149 @@ public:
     }
     // pop_front
     dataType & pop_front() {
-        dataType tmp = (*this)(0);
+        dataType tmp = (*this)[0];
         remove(0);
         return tmp;
     }
     // push_back a vector
-    void push_back(const AP_Vector & vector) {
+    void push_back(const Vector & vector) {
         for (size_t i=0; i<vector.getSize(); i++)
-            push_back(vector(i));
+            push_back(vector[i]);
     }
     // const array access operator
-    const dataType & operator()(const size_t & index) const {
+    const dataType & operator[](const size_t & index) const {
 #ifdef ASSERT
         assert(index<getSize(),vectorSource,__LINE__);
 #endif
         return data[index];
     }
     // array access operator
-    dataType & operator()(const size_t & index) {
+    dataType & operator[](const size_t & index) {
 #ifdef ASSERT
         assert(index<getSize(),vectorSource,__LINE__);
 #endif
         return data[index];
     }
     // assignment operator
-    AP_Vector & operator=(const AP_Vector & v) {
+    Vector & operator=(const Vector & v) {
         setSize(v.getSize());
         for (size_t i=0; i<getSize(); i++)
-            (*this)(i)=v(i);
+            (*this)[i]=v[i];
         return *this;
     }
     // equal
-    const bool operator==(const AP_Vector& v) const {
+    const bool operator==(const Vector& v) const {
 #ifdef ASSERT
         assert(getSize()==v.getSize(),vectorSource,__LINE__);
 #endif
         for (size_t i=0; i<getSize(); i++) {
-            if ((*this)(i)!=v(i)) return false;
+            if ((*this)[i]!=v[i]) return false;
         }
         return true;
     }
     // not equal
-    const bool operator!=(const AP_Vector& v) const {
+    const bool operator!=(const Vector& v) const {
         return !((*this)==v);
     }
     // addition
-    const AP_Vector operator+(const AP_Vector& v) const {
+    const Vector operator+(const Vector& v) const {
 #ifdef ASSERT
         assert(v.getSize() == getSize(),vectorSource,__LINE__);
 #endif
-        AP_Vector result(getSize());
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
-            result(i)=(*this)(i)+v(i);
+            result(i)=(*this)[i]+v[i];
         return result;
     }
     // addition
-    const AP_Vector operator+(const dataType& s) const {
-        AP_Vector result(getSize());
+    const Vector operator+(const dataType& s) const {
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
-            result(i)=(*this)(i)+s;
+            result[i]=(*this)[i]+s;
         return result;
     }
     // subtraction
-    const AP_Vector operator-(const AP_Vector& v) const {
+    const Vector operator-(const Vector& v) const {
 #ifdef ASSERT
         assert(v.getSize() == getSize(),vectorSource,__LINE__);
 #endif
-        AP_Vector result(getSize());
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
-            result(i)=(*this)(i)-v(i);
+            result[i]=(*this)[i]-v[i];
         return result;
     }
     // negation
-    const AP_Vector operator-() const {
-        AP_Vector result(getSize());
+    const Vector operator-() const {
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
-            result(i)=-(*this)(i);
+            result[i]=-(*this)[i];
         return result;
     }
     // +=
-    AP_Vector& operator+=(const AP_Vector& v) {
+    Vector& operator+=(const Vector& v) {
 #ifdef ASSERT
         assert(v.getSize() == getSize(),vectorSource,__LINE__);
 #endif
-        AP_Vector result(getSize());
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
             (*this)(i)+=v(i);
         return *this;
     }
     // -=
-    AP_Vector& operator-=( const AP_Vector& v) {
+    Vector& operator-=( const Vector& v) {
 #ifdef ASSERT
         assert(v.getSize() == getSize(),vectorSource,__LINE__);
 #endif
-        AP_Vector result(getSize());
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
             (*this)(i)-=v(i);
         return *this;
     }
     // elementwise  mult.
-    const AP_Vector operator*(const AP_Vector & v) const {
-        AP_Vector result(getSize());
+    const Vector operator*(const Vector & v) const {
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
             result(i)=(*this)(i)*v(i);
         return result;
     }
 
     // mult. by a scalar
-    const AP_Vector operator*(const dataType & s) const {
-        AP_Vector result(getSize());
+    const Vector operator*(const dataType & s) const {
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
             result(i)=(*this)(i)*s;
         return result;
     }
     // div. by a scalar
-    const AP_Vector operator/(const dataType & s) const {
-        AP_Vector result(getSize());
+    const Vector operator/(const dataType & s) const {
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
             result(i)=(*this)(i)/s;
         return result;
     }
     // elementwise div.
-    const AP_Vector operator/(const AP_Vector & v) const {
-        AP_Vector result(getSize());
+    const Vector operator/(const Vector & v) const {
+        Vector result(getSize());
         for (size_t i=0; i<getSize(); i++)
             result(i)=(*this)(i)/v(i);
         return result;
     }
 
     // div. by a scalar
-    AP_Vector & operator/=(const dataType & s) {
+    Vector & operator/=(const dataType & s) {
         for (size_t i=0; i<getSize(); i++)
             (*this)(i)/=s;
         return *this;
     }
     // mult. by a scalar
-    AP_Vector & operator*=(const dataType & s) {
+    Vector & operator*=(const dataType & s) {
         for (size_t i=0; i<getSize(); i++)
             (*this)(i)*=s;
         return *this;
     }
     // cross/vector product
-    const AP_Vector cross(const AP_Vector& v) const {
-        AP_Vector result(3), u=*this;
+    const Vector cross(const Vector& v) const {
+        Vector result(3), u=*this;
 #ifdef ASSERT
         assert(u.getSize()==3 && v.getSize()==3,vectorSource,__LINE__);
 #endif
@@ -287,7 +287,7 @@ public:
         return result;
     }
     // dot/scalar product
-    const dataType dot(const AP_Vector& v) const {
+    const dataType dot(const Vector& v) const {
 #ifdef ASSERT
         assert(getSize()==v.getSize(),vectorSource,__LINE__);
 #endif
@@ -300,7 +300,7 @@ public:
         return sqrt(dot(*this));
     }
     // unit vector
-    const AP_Vector unit() const {
+    const Vector unit() const {
         return (*this)*(1/norm());
     }
     // sum
@@ -323,8 +323,8 @@ public:
         }
     }
     // range
-    const AP_Vector range(const size_t & start, const size_t & stop) const {
-        AP_Vector v(stop-start+1);
+    const Vector range(const size_t & start, const size_t & stop) const {
+        Vector v(stop-start+1);
         for (size_t i=start; i<=stop; i++) v(i-start) = (*this)(i);
         return v;
     }
@@ -345,8 +345,8 @@ public:
     }
     // self test
     static bool selfTest(Stream & serial=Serial) {
-        serial.println("AP_Vector self test.");
-        AP_Vector u(3),v(3),w(3);
+        serial.println("Vector self test.");
+        Vector u(3),v(3),w(3);
         u(0) = 1;
         u(1) = 2;
         u(2) = 3;