aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added Loop class.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1251 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2010-12-24 23:59:10 +00:00
parent d6818f99c3
commit 98624b3de7
4 changed files with 192 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Common/AP_Common.h
View File

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

57
libraries/AP_Common/AP_Loop.cpp Normal file
View File

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

70
libraries/AP_Common/AP_Loop.h Normal file
View File

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

126
libraries/AP_Common/AP_Vector.h
View File

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