Kalman filter for Eurobot

Files at this revision

API Documentation at this revision

Comitter:
madcowswe
Date:
Tue Mar 20 12:43:16 2012 +0000
Commit message:
Algo done, vars not done

Changed in this revision

Kalman.cpp Show annotated file Show diff for this revision Revisions of this file
Kalman.h Show annotated file Show diff for this revision Revisions of this file
Matrix/Log.c Show annotated file Show diff for this revision Revisions of this file
Matrix/Matrix.cpp Show annotated file Show diff for this revision Revisions of this file
Matrix/Matrix.h Show annotated file Show diff for this revision Revisions of this file
Matrix/Operators.cpp Show annotated file Show diff for this revision Revisions of this file
MatrixMath/MatrixMath.cpp Show annotated file Show diff for this revision Revisions of this file
MatrixMath/MatrixMath.h Show annotated file Show diff for this revision Revisions of this file
RFSRF05/RF12B.cpp Show annotated file Show diff for this revision Revisions of this file
RFSRF05/RF12B.h Show annotated file Show diff for this revision Revisions of this file
RFSRF05/RFSRF05.cpp Show annotated file Show diff for this revision Revisions of this file
RFSRF05/RFSRF05.h Show annotated file Show diff for this revision Revisions of this file
RFSRF05/RFSerial.cpp Show annotated file Show diff for this revision Revisions of this file
RFSRF05/RFSerial.h Show annotated file Show diff for this revision Revisions of this file
RFSRF05/RF_defs.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r a0285293f6a6 Kalman.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman.cpp	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,92 @@
+//***************************************************************************************
+//Kalman Filter implementation
+//***************************************************************************************
+#include "Kalman.h"
+#include "RFSRF05.h"
+#include "MatrixMath.h"
+#include "Matrix.h"
+#include "math.h"
+
+#define PI 3.14159265
+
+Kalman::Kalman() :
+    X(3,1),
+    P(3,3),
+    Q(3,3),
+    sonararray(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9)  {
+
+    //Initilising matrices
+    X << 0
+      << 0
+      << 0;
+      
+    //Q << (insert numbers here)
+    //R = a number;
+    
+    //attach callback
+    sonararray.callbackobj = (DummyCT*)this;
+    sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance)) &Kalman::update;
+    
+}
+
+void Kalman::predict(float left, float right) {
+
+    float dxp, dyp;
+    
+    //The below calculation are in body frame (where +x is forward)
+    float thetap = (right - left) / (2.0f * PI * wheelbase);
+
+    if (thetap < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
+        float d = (right + left)/2.0f;
+        dxp = d*cos(thetap/2);
+        dyp = d*sin(thetap/2);
+    } else { //calculate circle arc
+        float r = (right + left) / (4.0f * PI * thetap);
+        dxp = r*sin(thetap);
+        dyp = r - r*cos(thetap);
+    }
+
+    //rotating to cartesian frame and updating state
+    X(1,1) += dxp * cos(X(3,1)) - dyp * sin(X(3,1));
+    X(2,1) += dxp * sin(X(3,1)) + dyp * cos(X(3,1));
+    X(3,1) += thetap;
+    
+    //Linearising F around X
+    Matrix F(3,3);
+    F << 1 << 0 << (dxp * -sin(X(3,1)) - dyp * cos(X(3,1)))
+      << 0 << 1 << (dxp * cos(X(3,1)) - dyp * sin(X(3,1)))
+      << 0 << 0 << 1;
+    
+    //Updating P
+    P = F * P * MatrixMath::Transpose(F) + Q;
+}
+
+void Kalman::update(int sonarid, float dist){
+
+    float rbx = X(1,1) - beaconx;
+    float rby = X(2,1) - beacony;
+    
+    float expecdist = sqrt(rbx*rbx + rby*rby);
+    float Y = dist - expecdist;
+    
+    float dhdx = rbx / expecdist;
+    float dhdy = rby / expecdist;
+    Matrix H(1,3);
+    H << dhdx << dhdy << 0;
+    
+    Matrix PH = P * MatrixMath::Transpose(H);    
+    float S = (H * PH)(1,1) + R;
+    Matrix K = PH * (1/S);
+    
+    //Updating state
+    X += K*Y;
+    
+    Matrix I3(3,3);
+    I3 << 1 << 0 << 0
+       << 0 << 1 << 0
+       << 0 << 0 << 1;
+    P = (I3 - K * H) * P;
+    
+    
+
+}
\ No newline at end of file
diff -r 000000000000 -r a0285293f6a6 Kalman.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman.h	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,27 @@
+#include "Matrix.h"
+#include "RFSRF05.h"
+
+//const int wheelmm = 314;
+//const int robotCircumference = 1052;
+//TODO make a globals and defines header file
+const int wheelbase = 400;
+
+class Kalman {
+public:
+    Kalman();
+
+    void predict(float left, float right);
+    void update(int sonarid, float dist);
+    
+    //State variables
+    Matrix X; //(3,1)
+    Matrix P; //(3,3);
+    
+private:
+    Matrix Q; //perhaps calculate on the fly? dependant on speed etc?
+    float R;
+    
+    RFSRF05 sonararray;//(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9);
+    static const float beaconx = 0;
+    static const float beacony = 0;
+};
\ No newline at end of file
diff -r 000000000000 -r a0285293f6a6 Matrix/Log.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix/Log.c	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,61 @@
+/**  
+ * @brief  Keep track of changes since version 1.6             e-mail: mecatronica.mid@gmail.com
+ * @file   Log.c
+ * @author Ernesto Palacios
+ */
+ 
+/*    
+1.6.4.0    30/10/2011
+           -->> MATRIX_H
+            *  Eliminated namespace std; already included in mbed.h
+            *  Operator Overloaded (). For assignment and getValue.
+            *  Almost all operators declared as friend functions.
+
+            -->> MATRIXMATH_H
+            *  Added Function Eye(). Creates an identity Matrix of specified dimensions.
+            *  Added Function dotProduct(). to find the dot product of two vectors.
+               -->> You need to pass two Vector Matrices of any dimmensions.
+                    They could be in the form: A( 1,n ) B( 1,n )
+                                               A( n,1 ) B( 1,n )
+                                               A( n,1 ) B( n,1 )
+                                               A( 1,n ) B( n,1 )
+                    As long as have same 'depth' and are vectors, the dot product.
+                    will be returned.
+
+            -->> MATRIXMATH_Kinematics.cpp
+            *  Created file MatrixMath_Kinematics.cpp To Hold the definitions of
+               kinematic operations.
+
+            *  Define Functions RotX, RotY, RotZ, Transl. for Matrix Transformation
+               operations.
+
+
+1.6.2.0     22/10/2011
+            -->> MATRIX_H
+            *  Changed static member Matrix::AddColumn( ... )  -> Matrix::AddCol( ... )
+            *  Overload AddCol/AddRow, it now can accept SingleCol/ SingleRow as arguments.
+               Still works the same for inserting new Col/Row. Usage:
+
+               Matrix::AddRow( myMatrix, 3 );  // Inserts an empty col at index 3 of myMatrix
+
+               Matrix::AddCol( myMatrix, SingleCol, 3 ); // Inserts a SingleCol Matrix into index 3 of myMarix
+
+            -->> MATRIXMATH_H
+            *  float det = MatrixMath::det( myMatrix );
+               Returns the determinant of any nxn Matrix.
+
+            *  Matrix Inv = MatrixMath::Inv( myMatrix )
+               Returns the determinant of any nxn Matrix, if it's not a Singular matrix
+
+
+               WARNING: If it is a Singular Matrix it will return the same Matrix.
+                        A singular Matrix is one whose inverse does not exists.
+
+1.6.0.1     21/10/2011
+            First class ready to work. but still some rough edges to polish. Better use 1.6.2
+
+1.0         15/09/2011
+
+            First Version.- Buggy and no longer supported.
+
+*/
diff -r 000000000000 -r a0285293f6a6 Matrix/Matrix.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix/Matrix.cpp	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,387 @@
+/**
+ * @brief  Source Code for the Matrix Class.
+ * @file   Matrix.cpp
+ * @author Ernesto Palacios
+ *
+ * Created on September 2011.
+ *
+ * Develop Under  GPL v3.0 License
+ * http://www.gnu.org/licenses/gpl-3.0.html
+ *
+ */
+
+#include "mbed.h"
+#include "Matrix.h"
+
+/// Rows by Cols Matrix Constructor
+Matrix::Matrix(int Rows, int Cols): _nRows(Rows), _nCols(Cols)
+{
+    _matrix.resize(_nRows);
+    for( int i = 0; i < _nRows; i++ )
+        _matrix[i].resize(_nCols);
+
+    _pRow = 0;
+    _pCol = 0;
+
+    this->Clear();  //Make all elements zero by default.
+}
+
+
+/// Copies one matrix into a new one
+Matrix::Matrix(const Matrix& base)
+{
+    _nCols = base._nCols;
+    _nRows = base._nRows;
+
+    _pRow  = base._pRow;
+    _pCol  = base._pCol;
+
+    _matrix.resize(_nRows);
+    for( int i = 0; i < _nRows; i++ )
+        _matrix[i].resize(_nCols);
+
+    for( int i = 0; i < _nRows; i++ )
+        for( int j = 0; j < _nCols; j++ )
+            _matrix[i][j] = base._matrix[i][j];
+}
+
+
+/// Default Constructor
+Matrix::Matrix()
+{
+    _nCols = 0;
+    _nRows = 0;
+
+    _pRow = 0;
+    _pCol = 0;
+
+}
+
+/***********************************************************************/
+
+/// Returns true if matrix is full of zeros
+bool Matrix::isZero() const
+{
+    bool zero = false;
+    for( int i = 0; i < this->_nRows; i++ )
+        for( int j = 0; j < this->_nCols; j++ )
+            if( _matrix[i][j] != 0 )
+                zero = zero || true;
+    return !zero;
+}
+
+
+/// Returns true if Matrix is Single Row ot Single Column.
+bool Matrix::isVector() const
+{
+    if( _nRows == 1 || _nCols == 1 )
+        return true;
+    else
+        return false;
+}
+
+/*************************************************************************/
+
+/// Returns all elements in Matrix as a single Row vector.
+const Matrix Matrix::ToPackedVector( const Matrix& Mat )
+{
+
+    Matrix Crushed( 1, Mat._nRows * Mat._nCols );
+
+    int cont = 0;
+
+    for( int i = 0; i < Mat._nRows; i++ )
+        for( int j = 0; j < Mat._nCols; j++ )
+        {
+            Crushed._matrix[0][cont] = Mat._matrix[i][j];
+            cont++;
+        }
+
+    Crushed._pRow = Crushed._nRows;
+    Crushed._pCol = Crushed._nCols;
+
+    return Crushed;
+}
+
+
+
+/// To add (Insert) a Single Row to a Matrix.
+void Matrix::AddRow(Matrix& Mat, int index)
+{
+    --index;
+
+    if( index > Mat._nRows + 1)
+    {
+        printf("\n\nERROR:\nRow out of Limits @ AddRow()\n");
+
+    }else{
+
+       Mat._nRows++;
+       Mat._matrix.resize( Mat._nRows );
+
+       Mat._matrix[ Mat._nRows - 1 ].resize( Mat._nCols );
+
+       for( int i = Mat._nRows - 1; i > index; i-- )
+           for( int j = 0; j < Mat._nCols; j++ )
+               Mat._matrix[i][j] = Mat._matrix[i - 1][j];
+
+       for( int j = 0; j < Mat._nCols; j++ )
+           Mat._matrix[index][j] = 0.0;
+    }
+}
+
+
+void Matrix::AddRow(Matrix& Receip, const Matrix& Row, int index)
+{
+    Matrix::AddRow( Receip, index );  //Make Room
+
+    --index;
+    for( int i = 0; i < Receip._nCols; i++ )
+        Receip._matrix[index][i] = Row._matrix[0][i];   //Copy Data.
+
+}
+
+
+/// To add (Insert) a single Column to a Matrix
+void Matrix::AddCol( Matrix& Mat, int index )
+{
+    --index;
+
+    if( index > Mat._nCols + 1 )
+    {
+        printf("\n\nERROR:\nRow out of Limits on AddCol()\n");
+
+    }else{
+
+
+            Mat._nCols++;
+            for( int i = 0; i < Mat._nRows; i++ )
+                Mat._matrix[i].resize( Mat._nCols );
+
+            for( int i = 0; i < Mat._nRows; i++ )
+                for( int j = Mat._nCols; j > index; j-- )
+                    Mat._matrix[i][j] = Mat._matrix[i][j - 1];
+
+            for( int i = 0; i < Mat._nRows; i++ )
+                Mat._matrix[i][index] = 0.0;
+
+    }
+}
+
+
+void Matrix::AddCol(Matrix& Receip, const Matrix& Row, int index)
+{
+    Matrix::AddCol( Receip, index ); // Make Rom
+
+    --index;
+    for( int i = 0; i < Receip._nRows; i++ )
+        Receip._matrix[i][index] = Row._matrix[i][0];   //Copy Data.
+}
+
+
+/// Delete a Single Column From Matrix.
+void Matrix::DeleteCol( Matrix& Mat, int Col)
+{
+    --Col; // Because of Column zero.
+
+    if( Col > Mat._nCols )
+    {
+        printf("\n\nERROR:\nColumn out of Limits @ DeleteCol()\n");
+
+    }else{
+
+        for( int i = 0; i < Mat._nRows; i++ )
+            for( int j = Col; j < Mat._nCols; j++ )
+                Mat._matrix[i][j] = Mat._matrix[i][j+1];
+
+        // If adressing last element of Column,
+        // wich no longer exists
+        if( Mat._pCol == Mat._nCols )
+            Mat._pCol--;
+
+        // Decrease one column
+        Mat._nCols--;
+
+        //Erase last Column
+        for( int i = 0; i < Mat._nRows; i++ )
+            Mat._matrix[i].reserve(Mat._nCols);
+
+    }
+}
+
+
+/// Delete a Single Row form Matrix
+void Matrix::DeleteRow(Matrix& Mat, int Row)
+{
+    --Row;
+
+    if( Row > Mat._nRows )
+    {
+        printf("\n\nERROR:\nColumn out of Limits @ DeleteCol()\n");
+
+    }else{
+
+        for( int i = Row; i < Mat._nRows - 1; i++ )
+
+            for( int j = 0; j < Mat._nCols; j++ )
+                Mat._matrix[i][j] = Mat._matrix[i+1][j];
+        Mat._nRows--;
+        Mat._matrix.resize(Mat._nRows);
+    }
+}
+
+/*****************************************************************************************/
+
+/// Extracts a single row form calling matrix and saves it to another matrix.
+const Matrix Matrix::ExportRow( const Matrix& Mat, int row )
+{
+    --row;
+
+    if( row > Mat._nRows )
+    {
+        printf( "\n\nERROR:\nRow out of dimmensions @ GetRow\n"
+                "Nothing Done.\n\n" );
+
+    }else{
+
+        Matrix SingleRow( 1 , Mat._nCols );
+        SingleRow.Clear();
+
+        for( int j = 0; j < Mat._nCols; j++ )
+        SingleRow._matrix[0][j] = Mat._matrix[row][j];
+
+        SingleRow._pCol = SingleRow._nCols;
+        SingleRow._pRow = 0;
+
+        return SingleRow;
+    }
+}
+
+
+/// Extracts a single column form calling matrix and saves it to another matrix.
+const Matrix Matrix::ExportCol( const Matrix& Mat, int col )
+{
+    --col;
+
+    if( col > Mat._nCols )
+    {
+        printf( "\n\nERROR:\nColumn out of dimmensions.\n"
+                "Nothing Done.\n\n" );
+    }else{
+
+        Matrix SingleCol( Mat._nRows, 1 );
+        for(int i = 0; i < Mat._nRows; i++ )
+            SingleCol._matrix[i][0] = Mat._matrix[i][col];
+
+        SingleCol._pCol = 0;
+        SingleCol._pRow = SingleCol._nRows;
+
+        return SingleCol;
+    }
+}
+
+
+/// Makes matrix Bigger!
+void Matrix::Resize( int Rows, int Cols )
+{
+    _nRows = Rows;  //Decreases one because internally
+    _nCols = Cols; // Index starts at zero.
+
+    _matrix.resize( _nRows );
+
+    for( int i = 0; i< _nRows ; i++ )
+        _matrix[i].resize(_nCols);
+
+    _pRow = 0; // If matrix is resized the <<
+    _pCol = 0; // operator overwrites everything!
+}
+
+
+/// Ask user for elemnts in Matrix
+void Matrix::FillMatrix()
+{
+    for(int i = 0; i < _nRows; i++)
+    {
+        for(int j = 0; j < _nCols; j++)
+        {
+            printf( "Position [%u][%u]: ", i, j );
+            float numero;
+            scanf( "%f", &numero );
+            printf("%.3f ", numero);
+            this->_matrix[i][j] = numero;
+        }
+        printf("\n");
+    }
+    printf("\n");
+
+    _pRow = _nRows;
+    _pCol = _nCols;
+}
+
+
+/// Prints out Matrix.
+void Matrix::print() const
+{
+    for( int i = 0; i < _nRows; i++ )
+    {
+        for( int j = 0; j < _nCols; j++ )
+        {
+            printf( "%.3f, ",_matrix[i][j] );
+
+        }
+        printf( "\n" );
+    }
+}
+
+
+/// Fills matrix with zeros.
+void Matrix::Clear()
+{
+    for( int i = 0; i < _nRows; i++ )
+       for( int j = 0; j < _nCols; j++ )
+           _matrix[i][j] = 0;
+
+    _pCol = 0;  // New data can be added
+    _pRow = 0;
+}
+
+/********************************************************************************/
+
+
+/// Inserts a Single element in a desired Position( Index starts at [1][1] );
+void Matrix::add(int Row, int Col, float number)
+{
+    --Col; --Row;
+
+    if( Row > _nRows || Col > _nCols )
+    {
+        printf("\n\nERROR:\nOut of limits of Matrix @ mat.Add()");
+
+    }else{
+        _matrix[Row][Col] = number;
+    }
+}
+
+
+/// Adds all elements in matrix and returns the answer.
+float Matrix::sum() const
+{
+    float total = 0;
+
+    for( int i = 0; i < _nRows; i++ )
+        for( int j = 0; j < _nCols; j++ )
+            total += _matrix[i][j];
+    return total;
+}
+
+
+/// Returns the specified element. Index Starts at [1][1].
+float Matrix::getNumber( int Row, int Col ) const
+{ return this->_matrix[Row -1][Col - 1]; }
+
+/// Returns the number of Rows in Matrix.
+int Matrix::getRows() const{ return this->_nRows; }
+
+
+/// Returns the number of Columns in Matrix.
+int Matrix::getCols() const{ return this->_nCols; }
\ No newline at end of file
diff -r 000000000000 -r a0285293f6a6 Matrix/Matrix.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix/Matrix.h	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,423 @@
+/**
+ * @brief  API for Matrix Library
+ * @file   Matrix.h
+ * @author Ernesto Palacios
+ *
+ * Created on 13 de septiembre de 2011, 03:49 PM
+ *
+ * Develop Under  GPL v3.0 License
+ * http://www.gnu.org/licenses/gpl-3.0.html
+ *
+ */
+
+#ifndef MATRIX_H
+#define MATRIX_H
+
+
+#include <vector>
+
+class MatrixMath;
+
+/**
+ * @brief This class provide basic manipulation for 2D matrices see Log.c for more info
+ * version 1.6.4.
+ *
+ */
+class Matrix{
+public:
+
+    /// Creates a nex Matrix of Size [ Row x Cols ]
+    Matrix( int Rows, int Cols );
+
+
+    /// Creates a new Matrix identical to an input Matrix
+    Matrix( const Matrix& base );
+
+
+    /// Default Constructor
+    Matrix();
+
+
+/******************************************************************************/
+
+
+    /**@brief This includes the Class to handle Matrix Operations.
+     */
+    friend class MatrixMath;
+
+
+    /**@brief
+     * Subindex for Matrix elements assignation.
+     * @param row
+     * @param col
+     * @return pointer to the element.
+     */
+    float& operator() ( int row, int col );
+
+
+    /**@brief
+     *Subindex for Matrix element.
+     * @param row
+     * @param col
+     * @return the element.
+     */
+    float  operator() ( int row, int col ) const;
+    
+    
+    
+    /** @brief
+     * Overwrites all data. To be used Carefully!
+     */
+    Matrix& operator = ( const Matrix& rightM );
+
+
+    /** @brief
+     * Overload opeartor for the compare Matrices
+     *
+     * @param rightM
+     * @return Boolean 'false' if different.
+     */
+    friend bool operator == ( const Matrix& leftM, const Matrix& rightM );
+
+
+    /** @brief
+     * Overload opeartor for the compare Matrices
+     *
+     * @param rightM
+     * @return Boolean 'true' if different
+     */
+    friend bool operator != ( const Matrix& leftM, const Matrix& rightM );
+
+
+    /** @brief
+     * Overload Copmpound assignment.
+     * @param rightM
+     * @return A new Matrix to be assigned to itself.
+     */
+    friend Matrix& operator += ( Matrix& leftM, const Matrix& rightM );
+
+
+    /** @brief
+     * Overload Compund decrease.
+     * @param rightM Right hand matrix
+     * @return A new Matrix to be assigned to itself
+     */
+    friend Matrix& operator -= ( Matrix& leftM, const Matrix& rightM );
+
+
+    /** @brief
+     * Overload Compound CrossProduct Matrix operation.
+     * @param rightM
+     * @return
+     */
+    friend Matrix& operator *=( Matrix& leftM, const Matrix& rightM );
+
+
+    /** @brief
+     * Overload Compund Element-by-elemnt scalar multiplication.
+     * @param number
+     * @return
+     */
+    friend Matrix& operator *=( Matrix& leftM, float number );
+
+
+
+    /**@brief
+     * All elements in matrix are multiplied by (-1).
+     * @return A new Matrix object with inverted values.
+     */
+    const Matrix operator -();
+
+
+    /**@brief
+     * Overload Compound add with scalar.
+     * Because the '=' operator checks for self Assign, no extra operations
+     * are needed.
+     * @return Same Matrix to self Assign.
+     */
+    friend const Matrix operator +=( Matrix& leftM, float number );
+
+
+    /**@brief
+     * Compound substract with scalar.
+     * @return Same matrix to self Assign.
+     */
+    friend const Matrix operator -=( Matrix& leftM, float number );
+
+
+    /** @brief
+     * Adds two matrices of the same dimensions, element-by-element.
+     * If diferent dimensions -> ERROR.
+     * @return A new object Matrix with the result.
+     */
+    friend const Matrix operator +( const Matrix& leftM, const Matrix& rightM);
+
+
+    /** @brief
+     * Adds the given nomber to each element of matrix.
+     * Mimic MATLAB operation.
+     * @return A new matrix object with the result.
+     */
+    friend const Matrix operator +( const Matrix& leftM, float number );
+
+
+
+    /**@brief
+     * Adds the given number to each element in Matrix.
+     * @return A new Matrix object with the result.
+     */
+    friend const Matrix operator +( float number, const Matrix& leftM );
+
+
+    /**@brief
+     * Substracts two matrices of the same size, element-by-element.
+     * If different dimensions -> ERROR.
+     * @return  A new object Matrix with the result.
+     */
+    friend const Matrix operator -( const Matrix& leftM, const Matrix& rightM );
+
+
+    /**@brief
+     * Substracts each element in Matrix by number.
+     * @return A new matrix object with the result.
+     */
+    friend const Matrix operator -( const Matrix& leftM, float number );
+
+
+    /**@brief
+     * Substracts each element in Matrix by number
+     * @return A new matrix object with the result.
+     */
+    friend const Matrix operator -( float number, const Matrix& leftM );
+
+
+    /**
+     * Preforms Crossproduct between two matrices.
+     * @return
+     */
+    friend const Matrix operator *( const Matrix& leftM, const Matrix& rightM );
+
+
+    /**@brief
+     * Multiplies a scalar number with each element on Matrix.
+     * @return A new object with the result.
+     */
+    friend const Matrix operator *( const Matrix& leftM, float number );
+
+
+    /**@brief
+     * Multiplies a scalar number with each element on Matrix.
+     * @return
+     */
+    friend const Matrix operator *( float number, const Matrix& leftM );
+
+
+    /**@brief
+     * Inputs numbres into a Matrix, the matrix needs to be costructed as
+     * Matrix( _nRows, _nCols ).
+     * This does NOT work on an only declared Matrix such as:
+     * Matrix obj;
+     * obj << 5; //Error
+     * @return
+     */
+    friend Matrix& operator <<( Matrix& leftM, float number );
+
+/***********************************************************************/
+
+    /** @brief
+     * Returns TRUE if the matrix is zero, FALSE otherwhise
+     * @param mat: Matrix to be tested
+     */
+    bool isZero() const;
+
+
+    /** @brief
+     * Determines weather a Matrix is a Single Column or Row.
+     */
+    bool isVector() const;
+
+
+    /** @brief
+     * Shatters the matrix into a single Row Vector.
+     * Important: Returns NEW matrix, does no modify existing one.
+     */
+    static const Matrix ToPackedVector( const Matrix& Mat );
+
+
+    /** @brief
+     * Invoking this static method will increase a Row in Mat in the desired
+     * position.
+     * The current Row will be moved down to allocate space, and all elements will
+     * be initialized to zero in the new row.
+     * @param Mat: Matrix in wich to insert a Row
+     * @param Row: Number of row to insert, starts with one, not zero.
+     */
+    static void AddRow( Matrix& Mat, int index );
+
+
+    /**@brief
+     * Adds to Receip a new Row from another Matrix in desired index.
+     * Must be same size.
+     * The Row matrix must be SingleRow Matrix, you can use ExportRow
+     * to extract a Row from another Matrix.
+     * @param Receip Matrix to be Modified.
+     * @param Row Row to be added.
+     * @param index position in wich to be added, _nRow + 1 last position.
+     */
+    static void AddRow( Matrix& Receip, const Matrix& Row, int index );
+
+
+    /** @brief
+     * Invoking this static method will increase a Column in Matrix in the
+     * desired Position.
+     * @param Mat: Matrix in wich to insert a Column
+     * @param Col: Number of column, strats with one, not zero.
+     */
+    static void AddCol( Matrix& Mat, int index );
+
+
+    /**@brief
+     * This will copy a Column Matrix into Receip in desired Position,
+     * Must be same size.
+     * The Col Matrix must be a SingleCol Matrix, you can use ExportCol
+     * to extract a Column from another Matrix.
+     * @param Receip Matrix to be modified.
+     * @param Column Data to be copied.
+     * @param index Postion in Receip Matrix .
+     */
+    static void AddCol( Matrix& Receip, const Matrix& Col, int index  );
+
+
+    /** @brief
+     * Static Function Deletes Row from Matrix, Static to prevent missuse
+     * @param Mat: Matrix to delete Row from
+     * @param Row: Number of Row (first Row = 1)
+     */
+    static void DeleteRow( Matrix& Mat, int Row );
+
+
+    /** @brief
+     * Static Function Deletes Column from Matrix, it's Static to prevent
+     * missuse.
+     * Print error and does nothing if out of limits.
+     * @param Col: Number of Col to delete (first Col = 1)
+     * @param Mat: Matrix to delete from.
+     */
+    static void DeleteCol( Matrix& Mat, int Col );
+
+
+     /** @brief
+     * This method extracts a Row from a Matrix and Saves it in Mat.
+     * If Row is out of the parameters it does nothing, but prints a warning.
+     * @param Row: number of row to extract elements. this->_nRows.
+     * @param Mat: Matrix to extract from.
+     * @return New Row Matrix.
+     */
+     static const Matrix ExportRow( const Matrix& Mat, int row );
+
+
+    /** @brief
+     * This method extracts a Column from a Matrix and returns the Column
+     * as a new Matrix.
+     * If Row is out of the parameters, it does nothing and prints a warning.
+     * @param Col: number of Column to extract elements. this->_nCols.
+     * @param Mat: Matrix to extract from.
+     * @return New Row Matrix.
+     */
+     static const Matrix ExportCol( const Matrix& Mat, int col );
+
+
+    /** @brief
+     * This function resizes the Matrix to fit new data or cropped it,
+     * operator << can overwrite entire Matrix.
+     *
+     * @param Rows: New Number of Rows
+     * @param Cols: New numbler of columns
+     */
+    void Resize( int Rows, int Cols );
+
+
+    /** @brief
+     * Asks user for numbers to fill the Matrix elements, one by one.
+     * It uses printf(); by default the USBTX, USBRX, 9600, 1N8.
+     */
+    virtual void FillMatrix();
+
+
+    /** @brief
+     * Prints the entire Matrix using standard PRINTF
+     */
+    virtual void print() const;
+
+
+    /** @brief
+     * Makes all values on Matrix object zero.
+     * Also make posible use the '<<' operator to add elements and keep
+     * track of last element added.
+     */
+    void Clear();
+
+
+    /** @brief
+     * Assigns a float number to the matrix in a specified position
+     * Index starts at [1][1].
+     *
+     * @param number:   Number to be set
+     * @param Row:      Row of Matrix
+     * @param Col:      Column of Matrix
+     */
+    void add( int Row, int Col, float number );
+
+
+    /** @brief
+     * Returns the sum of every cell in the Matrix.
+     */
+    float sum() const;
+
+
+    /** @brief
+     * Return the number in position [Row],[Col]
+     * @param Row = number of row in matrix
+     * @param Col = number of Col in matrix
+     * @return Num = float number in matrix
+     */
+    float getNumber( int Row, int Col ) const;
+
+
+    /**@brief
+     * Retuns the number of Columns in Matrix, index starts at 1.
+     */
+    int  getCols() const;
+
+
+    /**@brief
+     *Retruns the number of Rows in Matrix, index starts at 1.
+     */
+    int  getRows() const;
+
+
+private:
+
+    /** 2-D Vector Array*/
+    std::vector < std::vector<float> > _matrix;
+
+
+
+    /** Number of Rows in Matrix*/
+    int _nRows;
+
+    /**Number of Columns in Matrix*/
+    int _nCols;
+
+
+
+    /**Last Element Row position in Matrix*/
+    int _pRow;
+
+    /**Last Element Col position in Matrix*/
+    int _pCol;
+
+};
+
+#endif    /* MATRIX_H */
+
diff -r 000000000000 -r a0285293f6a6 Matrix/Operators.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix/Operators.cpp	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,320 @@
+/**
+ * @brief  Source Code for the Operator of Matrix Class.
+ * @file   Operators.cpp
+ * @author Ernesto Palacios
+ * 
+ * Created on September 2011.
+ *
+ * Develop Under  GPL v3.0 License
+ * http://www.gnu.org/licenses/gpl-3.0.html
+ * 
+ */
+#include "mbed.h"
+#include "Matrix.h"
+
+/// Subindex in Matrix left side
+float& Matrix::operator ()(int row, int col)
+{
+    --row; --col;
+
+    if( row >= _nRows || col >= _nCols)
+    {
+        printf("\n\nError:\nOut of limits @ Matrix::operator()\n");
+    }else{
+        return _matrix[row][col];
+    }
+}
+
+/// Subindex in Matrix right side
+float Matrix::operator ()(int row, int col) const
+{
+    --row; --col;
+
+    if( row >= _nRows || col >= _nCols)
+    {
+        printf("\n\nError:\nOut of limits @ Matrix::operator()\n");
+    }else{
+        return _matrix[row][col];
+    }
+}
+
+
+/// Overloaded Asign Operator. Resizes Matrix
+Matrix& Matrix::operator = ( const Matrix& rightM )
+{
+    if (this != &rightM )
+    {
+
+         _nRows = rightM._nRows;
+         _nCols = rightM._nCols;
+
+         _matrix.resize( rightM._nRows );
+         for( int i = 0; i < rightM._nRows; i++ )
+             _matrix [i].resize(rightM._nCols);
+
+         for( int i = 0; i < _nRows; i++ )
+             for( int j = 0; j < _nCols; j++ )
+                 _matrix[i][j] = rightM._matrix[i][j];
+    }
+     return *this;
+
+}
+
+
+const Matrix Matrix::operator -()
+{
+    Matrix result( _nRows, _nCols );
+
+     for( int i = 0; i < _nRows; i++ )
+           for( int j = 0; j < _nCols; j++ )
+              result._matrix[i][j] = _matrix[i][j] * -1;
+
+    return result;
+}
+
+
+/// Comapre element by element
+bool operator == ( const Matrix& leftM, const Matrix& rightM )
+{
+    if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
+    {
+        bool equal = false;
+
+        for( int i = 0; i < leftM._nRows; i++ )
+            for( int j = 0; j < leftM._nCols; j++ )
+                if( leftM._matrix[i][j] != rightM._matrix[i][j] )
+                    equal = equal || true;
+
+        return !equal;
+
+    }else{  return false;  }
+}
+
+
+/// Calls for '==' operator
+bool operator != ( const Matrix& leftM, const Matrix& rightM )
+{
+    return !( leftM == rightM );
+}
+
+
+/// Matrices must be same size.
+/// Element by element adition.
+Matrix& operator +=( Matrix& leftM, const Matrix& rightM )
+{
+    if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
+    {
+        for( int i = 0; i < leftM._nRows; i++ )
+            for( int j = 0; j < leftM._nCols; j++ )
+                leftM._matrix[i][j] += rightM._matrix[i][j];
+
+        return leftM;
+
+    }else{ printf( "\n\nERROR:\nDiferent Dimensions @ += operator\n" );  }
+}
+
+
+/// Matrices must be same size.
+/// Element by element Substraction
+Matrix& operator -=( Matrix& leftM, const Matrix& rightM )
+{
+    if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
+    {
+        for( int i = 0; i < leftM._nRows; i++ )
+            for( int j = 0; j < leftM._nCols; j++ )
+                leftM._matrix[i][j] -= rightM._matrix[i][j];
+
+        return leftM;
+
+    }else{
+        printf( "\n\nERROR:\nDiferent Dimensions @ -= operator\n" );
+    }
+}
+
+
+Matrix& operator *=( Matrix& leftM, const Matrix& rightM )
+{
+    if( leftM._nCols == rightM._nRows )
+    {
+        Matrix resultM ( leftM._nRows, rightM._nCols );
+
+        for( int i = 0; i < resultM._nRows; i++ )
+            for( int j = 0; j < resultM._nCols; j++ )
+                for( int m = 0; m < rightM._nRows; m++ )
+                    resultM._matrix[i][j] += leftM._matrix[i][m] * rightM._matrix[m][j];
+
+        return resultM;
+    }else{
+        printf( "\n\nERROR:\nDiferent Dimensions @ *= operator\n" );
+    }
+}
+
+
+Matrix& operator *=( Matrix& leftM, float number )
+{
+    for( int i = 0; i < leftM._nRows; i++ )
+            for( int j = 0; j < leftM._nCols; j++ )
+                leftM._matrix[i][j] *= number;
+
+    return leftM;
+}
+
+
+/*****************************************************************************/
+
+// Overload operators
+
+
+const Matrix operator +=( Matrix& leftM, float number )
+{
+    for( int i = 0; i < leftM._nRows; i++ )
+           for( int j = 0; j < leftM._nCols; j++ )
+              leftM._matrix[i][j] += number;
+    return leftM;
+}
+
+
+const Matrix operator -=( Matrix& leftM, float number )
+{
+    for( int i = 0; i < leftM._nRows; i++ )
+           for( int j = 0; j < leftM._nCols; j++ )
+              leftM._matrix[i][j] -= number;
+    return leftM;
+}
+
+
+const Matrix operator +( const Matrix& leftM, const Matrix& rightM)
+{
+    if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
+    {
+        Matrix result( leftM._nRows, leftM._nCols );
+
+        for( int i = 0; i < leftM._nRows; i++ )
+           for( int j = 0; j < leftM._nCols; j++ )
+              result._matrix[i][j] = leftM._matrix[i][j] + rightM._matrix[i][j];
+
+        return result;
+
+    }else{
+        printf( "\n\nERROR\nDiferent Dimensions @ + operator \n" );
+        //Matrix error(4);
+        //error.Clear();
+        //return error;
+    }
+}
+
+
+const Matrix operator +( const Matrix& leftM, float number )
+{
+    Matrix result( leftM._nRows, leftM._nCols );
+
+    for( int i = 0; i < leftM._nRows; i++ )
+        for( int j = 0; j < leftM._nCols; j++ )
+            result._matrix[i][j] = leftM._matrix[i][j] + number;
+
+    return result;
+}
+
+
+const Matrix operator +( float number, const Matrix& leftM )
+{
+    return ( leftM + number );
+}
+
+
+const Matrix operator -( const Matrix& leftM, const Matrix& rightM )
+{
+    if( leftM._nRows == rightM._nRows  &&  leftM._nCols == rightM._nCols )
+    {
+        Matrix result( leftM._nRows, leftM._nCols );
+
+        for( int i = 0; i < leftM._nRows; i++ )
+           for( int j = 0; j < leftM._nCols; j++ )
+              result._matrix[i][j] = leftM._matrix[i][j] - rightM._matrix[i][j];
+
+        return result;
+
+    }else{
+        printf( "\n\nERROR:\nDiferent Dimensions @ + operator \n" );
+
+    }
+}
+
+
+const Matrix operator -( const Matrix& leftM, float number )
+{
+    Matrix result( leftM._nRows, leftM._nCols );
+
+    for( int i = 0; i < leftM._nRows; i++ )
+       for( int j = 0; j < leftM._nCols; j++ )
+          result._matrix[i][j] = leftM._matrix[i][j] - number;
+
+    return result;
+}
+
+
+const Matrix operator -( float number, const Matrix& leftM )
+{
+    return ( leftM - number );
+}
+
+
+const Matrix operator *( const Matrix& leftM, const Matrix& rightM )
+{
+    if( leftM._nCols == rightM._nRows )
+    {
+        Matrix resultM ( leftM._nRows, rightM._nCols );
+        resultM.Clear();
+
+        for( int i = 0; i < resultM._nRows; i++ )
+            for( int j = 0; j < resultM._nCols; j++ )
+                for( int m = 0; m < rightM._nRows; m++ )
+                    resultM._matrix[i][j] += leftM._matrix[i][m] * rightM._matrix[m][j];
+
+        return resultM;
+
+    } else {
+
+        printf("\n\nERROR:\nDiferent Dimension matrices @ * operator");
+    }
+
+}
+
+
+const Matrix operator *( const Matrix& leftM, float number )
+{
+    Matrix result( leftM._nRows, leftM._nCols );
+
+    for( int i = 0; i < leftM._nRows; i++ )
+       for( int j = 0; j < leftM._nCols; j++ )
+          result._matrix[i][j] = leftM._matrix[i][j] * number;
+
+    return result;
+}
+
+const Matrix operator *( float number, const Matrix& leftM )
+{
+    return ( leftM * number );
+}
+
+
+Matrix& operator <<( Matrix& leftM, float number )
+{
+    if( leftM._pCol == leftM._nCols ) //end of Row
+    {
+        leftM._pCol = 0;
+        leftM._pRow++;
+    }
+    if( leftM._pRow > leftM._nRows )
+    {
+        printf( "\n\nERROR:\nAssignment out of limits @ << operator" );
+        return leftM;
+
+    }else{
+
+        leftM._matrix[ leftM._pRow ][ leftM._pCol ] = number;
+        leftM._pCol++;
+
+        return leftM;
+    }
+}
diff -r 000000000000 -r a0285293f6a6 MatrixMath/MatrixMath.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MatrixMath/MatrixMath.cpp	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,175 @@
+/**
+ * @brief  Still under work  version 0.2
+ * @file   MatrixMath.cpp
+ * @author Erneseto Palacios
+ *
+ *  Develop Under  GPL v3.0 License
+ * http://www.gnu.org/licenses/gpl-3.0.html
+ */
+
+#include "mbed.h"
+#include "MatrixMath.h"
+
+///Transpose matrix
+Matrix MatrixMath::Transpose(const Matrix& Mat)
+{
+    Matrix result( Mat._nCols, Mat._nRows ); //Transpose Matrix
+
+    for( int i = 0; i < result._nRows; i++ )
+        for( int j = 0; j < result._nCols; j++ )
+            result._matrix[i][j] = Mat._matrix[j][i];
+
+    return result;
+}
+
+Matrix MatrixMath::Inv(const Matrix& Mat)
+{
+    if( Mat._nRows == Mat._nCols )
+    {
+        if( Mat._nRows == 2 )   // 2x2 Matrices
+        {
+            float det = MatrixMath::det( Mat );
+            if( det != 0 )
+            {
+                Matrix Inv(2,2);
+                Inv._matrix[0][0] =  Mat._matrix[1][1];
+                Inv._matrix[1][0] = -Mat._matrix[1][0];
+                Inv._matrix[0][1] = -Mat._matrix[0][1];
+                Inv._matrix[1][1] =  Mat._matrix[0][0] ;
+
+                Inv *= 1/det;
+
+                return Inv;
+
+            }else{
+                printf( "\n\nWANRING: same matrix returned");
+                printf( "\nSingular Matrix, cannot perform Invert @matrix " );
+//                Mat.print();
+                printf( "\n  _____________\n" );
+
+                return Mat;
+            }
+
+        }else{   // nxn Matrices
+
+            float det = MatrixMath::det( Mat );
+            if( det!= 0 )
+            {
+                Matrix Inv( Mat ); //
+                Matrix SubMat;
+
+                // Matrix of Co-factors
+                for( int i = 0; i < Mat._nRows; i++ )
+                    for( int j = 0; j < Mat._nCols; j++ )
+                    {
+                        SubMat = Mat ;
+
+                        Matrix::DeleteRow( SubMat, i+1 );
+                        Matrix::DeleteCol( SubMat, j+1 );
+
+                        if( (i+j)%2 == 0 )
+                            Inv._matrix[i][j] = MatrixMath::det( SubMat );
+                        else
+                            Inv._matrix[i][j] = -MatrixMath::det( SubMat );
+                    }
+
+                // Adjugate Matrix
+                Inv = MatrixMath::Transpose( Inv );
+
+                // Inverse Matrix
+                Inv = 1/det * Inv;
+
+                return Inv;
+
+            }else{
+                printf( "\n\nWANRING: same matrix returned");
+                printf( "\nSingular Matrix, cannot perform Invert @matrix " );
+          //      Mat.print();
+                printf( "\n  _____________\n" );
+
+                return Mat;
+            }
+
+        }
+
+    }else{
+        printf( "\n\nERROR:\nMust be square Matrix @ MatrixMath::Determinant " );
+    }
+}
+
+float MatrixMath::det(const Matrix& Mat)
+{
+    if( Mat._nRows == Mat._nCols  )
+    {
+
+        if( Mat._nRows == 2 )  // 2x2 Matrix
+        {
+            float det;
+            det = Mat._matrix[0][0] * Mat._matrix[1][1] -
+                    Mat._matrix[1][0] * Mat._matrix[0][1];
+            return det;
+        }
+        else if( Mat._nRows == 3 ) // 3x3 Matrix
+        {
+            float det;
+            MatrixMath dummy;
+
+            det = dummy.Det3x3( Mat );
+            return det;
+
+        } else {
+
+            float part1= 0;
+            float part2= 0;
+
+            //Find +/- on First Row
+            for( int i = 0; i < Mat._nCols; i++)
+            {
+                Matrix reduced( Mat );           // Copy Original Matrix
+                Matrix::DeleteRow( reduced, 1); // Delete First Row
+
+                if( i%2 == 0 ) //Odd Rows
+                {
+
+                    Matrix::DeleteCol( reduced, i+1);
+                    part1 += Mat._matrix[0][i] * MatrixMath::det(reduced);
+                }
+                else  // Even Rows
+                {
+                    Matrix::DeleteCol( reduced, i+1);
+                    part2 += Mat._matrix[0][i] * MatrixMath::det(reduced);
+                }
+            }
+            return part1 - part2; //
+        }
+
+    }else{
+        printf("\n\nERROR:\nMatrix must be square Matrix @ MatrixMath::Det");
+    }
+}
+
+
+/************************************/
+
+//Private Functions
+
+/**@brief
+ * Expands the Matrix adding first and second column to the Matrix then
+ * performs the Algorithm.
+ * @param Mat
+ * @return Determinant
+ */
+float MatrixMath::Det3x3(const Matrix& Mat)
+{
+    Matrix D( Mat );  //Copy Initial matrix
+
+    Matrix::AddCol(D, Matrix::ExportCol(Mat, 1), 4); //Repeat First Column
+    Matrix::AddCol(D, Matrix::ExportCol(Mat, 2), 5); //Repeat Second Column
+
+    float det = 0;
+    for( int i = 0; i < 3; i++ )
+        det += D._matrix[0][i] * D._matrix[1][1+i] * D._matrix[2][2+i]
+                - D._matrix[0][2+i] * D._matrix[1][1+i] * D._matrix[2][i];
+
+    return det;
+}
\ No newline at end of file
diff -r 000000000000 -r a0285293f6a6 MatrixMath/MatrixMath.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MatrixMath/MatrixMath.h	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,78 @@
+/**
+ * @file   MatrixMath.h
+ * @author Ernesto Palacios
+ *
+ * Created on 15 de septiembre de 2011, 09:44 AM.
+ *
+ *  Develop Under  GPL v3.0 License
+ *  http://www.gnu.org/licenses/gpl-3.0.html
+ *
+ */
+
+#ifndef    MATRIXMATH_H
+#define    MATRIXMATH_H
+
+#include "mbed.h"
+#include "Matrix.h"
+
+
+/**
+ * @brief This class provides STATIC methods to preform operations over
+ * Matrix Objects
+ * version 0.8.
+ *
+ * Methods will be added as neccesary.
+ *
+ */
+class MatrixMath{
+public:
+
+
+    /**@brief
+     * Transposes Matrix, return new Object.
+     * @param Mat matrix to calculate
+     * @return the determinant
+     */
+    static Matrix Transpose( const Matrix& Mat );
+
+
+    /**@brief
+     * Calculate the inverse of a nxn Matrix BUT check first if the determinant
+     * is != 0. Same matrix will be return if Det( Mat ) == 0.
+     * @param Mat matrix to calcute inverse.
+     * @return Matrix Inverse
+     */
+    static Matrix Inv( const Matrix& Mat );
+
+    
+    static float dotProduct( const Matrix& leftM, const Matrix& rightM );
+
+    /**@brief Calculates the determinant of a Matrix.
+     * @param Mat matrix to calculate.
+     * @return the determinant.
+     */
+    static float det( const Matrix& Mat );
+
+
+    //**  For Kinematics **//
+
+    static Matrix RotX( const Matrix& matrix, float radians );
+
+    static Matrix RotY( const Matrix& matrix, float radians );
+
+    static Matrix RotZ( const Matrix& matrix, float radians );
+
+    static Matrix Transl( const Matrix& matrix, float x, float y, float z );
+
+private:
+
+    /**@brief
+     * Calculates the Determinant of a 3x3 Matrix
+     * @param Mat Already made sure is a 3 by 3
+     * @return Float, determinant.
+     */
+    float Det3x3( const Matrix& Mat );
+
+};
+
+#endif    /* MATRIXMATH_H */
diff -r 000000000000 -r a0285293f6a6 RFSRF05/RF12B.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFSRF05/RF12B.cpp	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,380 @@
+#include "RF12B.h"
+
+#include "RF_defs.h"
+#include <algorithm>
+
+
+RF12B::RF12B(PinName _SDI,
+             PinName _SDO,
+             PinName _SCK,
+             PinName _NCS,
+             PinName _NIRQ):spi(_SDI, _SDO, _SCK),
+        NCS(_NCS), NIRQ(_NIRQ), NIRQ_in(_NIRQ), rfled(LED3) {
+
+    /* SPI frequency, word lenght, polarity and phase */
+    spi.format(16,0);
+    spi.frequency(2000000);
+
+    /* Set ~CS high */
+    NCS = 1;
+
+    /* Initialise RF Module */
+    init();
+
+    /* Setup interrupt to happen on falling edge of NIRQ */
+    NIRQ.fall(this, &RF12B::rxISR);
+}
+
+/* Returns the packet length if data is available in the receive buffer, 0 otherwise*/
+unsigned int RF12B::available() {
+    return fifo.size();
+}
+
+/* Reads a packet of data, with length "size" Returns false if read failed. TODO: make a metafifo to isolate packets*/
+bool RF12B::read(unsigned char* data, unsigned int size) {
+    if (fifo.size() == 0) {
+        return false;
+    } else {
+        unsigned int i = 0;
+        while (fifo.size() > 0 && i < size) {
+            data[i++] = fifo.front();
+            fifo.pop();
+        }
+        return true;
+    }
+}
+
+/* Reads a byte of data from the receive buffer */
+unsigned char RF12B::read() {
+    if (available()) {
+        unsigned char data = fifo.front();
+        fifo.pop();
+        return data;
+    } else {
+        return 0xFF; // Error val although could also be data...
+    }
+}
+
+/* Sends a packet of data to the RF module for transmission TODO: Make asych*/
+void RF12B::write(unsigned char *data, unsigned char length) {
+    unsigned char crc = 0;
+       
+    /* Transmitter mode */
+    changeMode(TX);
+
+    writeCmd(0x0000);
+    send(0xAA); // PREAMBLE
+    send(0xAA);
+    send(0xAA);
+    send(0x2D); // SYNC
+    send(0xD4);
+    /* Packet Length */
+    send(length);
+    crc = crc8(crc, length);
+    send(crc);
+    crc = crc8(crc, crc);
+    /* Packet Data */
+    for (unsigned char i=0; i<length; i++) {
+        send(data[i]);
+        crc = crc8(crc, data[i]);
+    }
+    send(crc);
+    send(0xAA); // DUMMY BYTES
+    send(0xAA);
+    send(0xAA);
+
+    /* Back to receiver mode */
+    changeMode(RX);
+    status();
+    
+
+}
+
+/* Transmit a 1-byte data packet */
+void RF12B::write(unsigned char data) {
+    write(&data, 1);
+}
+
+void RF12B::write(queue<char> &data, int length) {
+    char crc = 0;
+    char length_byte = 0;
+    
+    /* -1 means try to transmit everything in the queue */
+    if(length == -1) {
+        length = data.size();
+    }
+    
+    /* max length of packet is 255 */
+    length_byte = min(length, 255);
+    
+    /* Transmitter mode */
+    changeMode(TX);
+
+    writeCmd(0x0000);
+    send(0xAA); // PREAMBLE
+    send(0xAA);
+    send(0xAA);
+    send(0x2D); // SYNC
+    send(0xD4);
+    /* Packet Length */
+    send(length_byte);
+    crc = crc8(crc, length_byte);
+    send(crc);
+    crc = crc8(crc, crc);
+    /* Packet Data */
+    for (char i=0; i<length_byte; i++) {
+        send(data.front());
+        crc = crc8(crc, data.front());
+        data.pop();
+    }
+    send(crc);
+    send(0xAA); // DUMMY BYTES
+    send(0xAA);
+    send(0xAA);
+
+    /* Back to receiver mode */
+    changeMode(RX);
+    status();
+}
+
+/**********************************************************************
+ *  PRIVATE FUNCTIONS
+ *********************************************************************/
+
+/* Initialises the RF12B module */
+void RF12B::init() {
+    /* writeCmd(0x80E7); //EL,EF,868band,12.0pF
+     changeMode(RX);
+     writeCmd(0xA640); //frequency select
+     writeCmd(0xC647); //4.8kbps
+     writeCmd(0x94A0); //VDI,FAST,134kHz,0dBm,-103dBm
+     writeCmd(0xC2AC); //AL,!ml,DIG,DQD4
+     writeCmd(0xCA81); //FIFO8,SYNC,!ff,DR
+     writeCmd(0xCED4); //SYNC=2DD4
+     writeCmd(0xC483); //@PWR,NO RSTRIC,!st,!fi,OE,EN
+     writeCmd(0x9850); //!mp,90kHz,MAX OUT
+     writeCmd(0xCC17); //OB1, COB0, LPX, Iddy, CDDIT&#65533;CBW0
+     writeCmd(0xE000); //NOT USED
+     writeCmd(0xC800); //NOT USED
+     writeCmd(0xC040); //1.66MHz,2.2V */
+
+    writeCmd(
+        RFM_CONFIG_EL           |
+        RFM_CONFIG_EF           |
+        RFM_CONFIG_BAND_433     //|
+        //RFM_CONFIG_X_11_0pf // meh, using default
+    );
+
+    // 2. Power Management Command
+    // leave everything switched off for now
+    /*
+    writeCmd(
+        RFM_POWER_MANAGEMENT     // switch all off
+    );
+    */
+
+    // 3. Frequency Setting Command
+    writeCmd(
+        RFM_FREQUENCY            |
+        RFM_FREQ_433Band(435.7)  //I totally made this value up... if someone knows where the sweetspots are in this band, tell me!
+    );
+
+
+    // 4. Data Rate Command
+    writeCmd(RFM_DATA_RATE_9600);
+
+
+    // 5. Receiver Control Command
+    writeCmd(
+        RFM_RX_CONTROL_P20_VDI  |
+        RFM_RX_CONTROL_VDI_FAST |
+        //RFM_RX_CONTROL_BW(RFM_BAUD_RATE) |
+        RFM_RX_CONTROL_BW_134   |     // CHANGE THIS TO 67 TO IMPROVE RANGE! (though the bitrate must then be below 8kbaud, and fsk modulation changed)
+        RFM_RX_CONTROL_GAIN_0   |
+        RFM_RX_CONTROL_RSSI_103      // Might need adjustment. Datasheet says around 10^-5 bit error rate at this level and baudrate.
+    );
+
+    // 6. Data Filter Command
+    writeCmd(
+        RFM_DATA_FILTER_AL      |
+        RFM_DATA_FILTER_ML      |
+        RFM_DATA_FILTER_DIG     //|
+        //RFM_DATA_FILTER_DQD(4)
+    );
+
+    // 7. FIFO and Reset Mode Command
+    writeCmd(
+        RFM_FIFO_IT(8) |
+        RFM_FIFO_DR    |
+        0x8 //turn on 16bit sync word
+    );
+
+    // 8. FIFO Syncword
+    // Leave as default: 0xD4
+
+    // 9. Receiver FIFO Read
+    // when the interupt goes high, (and if we can assume that it was a fifo fill interrupt) we can read a byte using:
+    // result = RFM_READ_FIFO();
+
+    // 10. AFC Command
+    writeCmd(
+        //RFM_AFC_AUTO_VDI        |  //Note this might be changed to improve range. Refer to datasheet.
+        RFM_AFC_AUTO_INDEPENDENT    |
+        RFM_AFC_RANGE_LIMIT_7_8     |
+        RFM_AFC_EN                  |
+        RFM_AFC_OE                  |
+        RFM_AFC_FI
+    );
+
+    // 11. TX Configuration Control Command
+    writeCmd(
+        RFM_TX_CONTROL_MOD_60 |
+        RFM_TX_CONTROL_POW_0
+    );
+
+
+    // 12. PLL Setting Command
+    writeCmd(
+        0xCC77 & ~0x01 // Setting the PLL bandwith, less noise, but max bitrate capped at 86.2
+        // I think this will slow down the pll's reaction time. Not sure, check with someone!
+    );
+
+    changeMode(RX);
+    resetRX();
+    status();
+}
+
+/* Write a command to the RF Module */
+unsigned int RF12B::writeCmd(unsigned int cmd) {
+    NCS = 0;
+    unsigned int recv = spi.write(cmd);
+    NCS = 1;
+    return recv;
+}
+
+/* Sends a byte of data across RF */
+void RF12B::send(unsigned char data) {
+    while (NIRQ);
+    writeCmd(0xB800 + data);
+}
+
+/* Change the mode of the RF module to Transmitting or Receiving */
+void RF12B::changeMode(rfmode_t _mode) {
+    mode = _mode;
+    if (_mode == TX) {
+        writeCmd(0x8239); //!er,!ebb,ET,ES,EX,!eb,!ew,DC
+    } else { /* mode == RX */
+        writeCmd(0x8299); //er,!ebb,ET,ES,EX,!eb,!ew,DC
+    }
+}
+
+/* Interrupt routine for data reception */
+void RF12B::rxISR() {
+    unsigned int data = 0;
+    static int i = -2;
+    static unsigned char packet_length = 0;
+    static unsigned char crc = 0;
+    static queue<unsigned char> temp;
+
+    //Loop while interrupt is asserted
+    while (!NIRQ_in && mode == RX) {
+
+        /* Grab the packet's length byte */
+        if (i == -2) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                packet_length = (data&0x00FF);
+                crc = crc8(crc, packet_length);
+                i++;
+            }
+        }
+
+        //If we exhaust the interrupt, exit
+        if (NIRQ_in)
+            break;
+
+        // Check that packet length was correct
+        if (i == -1) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                unsigned char crcofsize = (data&0x00FF);
+                if (crcofsize != crc) {
+                    //It was wrong, start over
+                    i = -2;
+                    packet_length = 0;
+                    crc = 0;
+                    temp = queue<unsigned char>();
+                    resetRX();
+                } else {
+                    crc = crc8(crc, crcofsize);
+                    i++;
+                }
+            }
+        }
+
+        //If we exhaust the interrupt, exit
+        if (NIRQ_in)
+            break;
+
+        /* Grab the packet's data */
+        if (i >= 0 && i < packet_length) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                temp.push(data&0x00FF);
+                crc = crc8(crc, (unsigned char)(data&0x00FF));
+                i++;
+            }
+        }
+
+        //If we exhaust the interrupt, exit
+        if (NIRQ_in)
+            break;
+
+        if (i >= packet_length) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                if ((unsigned char)(data & 0x00FF) == crc) {
+                    //If the checksum is correct, add our data to the end of the output buffer
+                    while (!temp.empty()) {
+                        fifo.push(temp.front());
+                        temp.pop();
+                    }
+                }
+
+                /* Tell RF Module we are finished, and clean up */
+                i = -2;
+                packet_length = 0;
+                crc = 0;
+                temp = queue<unsigned char>();
+                resetRX();
+            }
+        }
+    }
+}
+
+unsigned int RF12B::status() {
+    return writeCmd(0x0000);
+}
+
+/* Tell the RF Module this packet is received and wait for the next */
+void RF12B::resetRX() {
+    writeCmd(0xCA81);
+    writeCmd(0xCA83);
+};
+
+/* Calculate CRC8 */
+unsigned char RF12B::crc8(unsigned char crc, unsigned char data) {
+    crc = crc ^ data;
+    for (int i = 0; i < 8; i++) {
+        if (crc & 0x01) {
+            crc = (crc >> 1) ^ 0x8C;
+        } else {
+            crc >>= 1;
+        }
+    }
+    return crc;
+}
\ No newline at end of file
diff -r 000000000000 -r a0285293f6a6 RFSRF05/RF12B.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFSRF05/RF12B.h	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,75 @@
+#ifndef _RF12B_H
+#define _RF12B_H
+
+#include "mbed.h"
+#include <queue>
+
+enum rfmode_t{RX, TX};
+
+class RF12B {
+public:
+    /* Constructor */
+    RF12B(PinName SDI,
+          PinName SDO,
+          PinName SCK,
+          PinName NCS,
+          PinName NIRQ);
+          
+    
+          
+    /* Reads a packet of data. Returns false if read failed. Use available() to check how much space to allocate for buffer */
+    bool read(unsigned char* data, unsigned int size);
+
+    /* Reads a byte of data from the receive buffer 
+        Returns 0xFF if there is no data */
+    unsigned char read();
+
+    /* Transmits a packet of data */
+    void write(unsigned char* data, unsigned char length);
+    void write(unsigned char data); /* 1-byte packet */
+    void write(queue<char> &data, int length = -1); /* sends a whole queue */
+    
+    /* Returns the packet length if data is available in the receive buffer, 0 otherwise*/
+    unsigned int available();
+
+protected:
+    /* Receive FIFO buffer */
+    queue<unsigned char> fifo;
+    
+    /* SPI module */
+    SPI spi;
+    
+    /* Other digital pins */
+    DigitalOut NCS;
+    InterruptIn NIRQ;
+    DigitalIn NIRQ_in;
+    DigitalOut rfled;
+    
+    rfmode_t mode;
+
+    /* Initialises the RF12B module */
+    void init();
+
+    /* Write a command to the RF Module */
+    unsigned int writeCmd(unsigned int cmd);
+    
+    /* Sends a byte of data across RF */
+    void send(unsigned char data);
+    
+    /* Switch module between receive and transmit modes */
+    void changeMode(rfmode_t mode);
+    
+    /* Interrupt routine for data reception */
+    void rxISR();
+    
+    /* Tell the RF Module this packet is received and wait for the next */
+    void resetRX();
+    
+    /* Return the RF Module Status word */
+    unsigned int status();
+    
+    /* Calculate CRC8 */
+    unsigned char crc8(unsigned char crc, unsigned char data);
+};
+
+#endif /* _RF12B_H */
\ No newline at end of file
diff -r 000000000000 -r a0285293f6a6 RFSRF05/RFSRF05.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFSRF05/RFSRF05.cpp	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,140 @@
+/* mbed SRF05 Ultrasonic Rangefiner Library
+ * Copyright (c) 2007-2010, cstyles, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "RFSRF05.h"
+#include "mbed.h"
+
+RFSRF05::RFSRF05(PinName trigger,
+                 PinName echo0,
+                 PinName echo1,
+                 PinName echo2,
+                 PinName echo3,
+                 PinName echo4,
+                 PinName echo5,
+                 PinName SDI,
+                 PinName SDO,
+                 PinName SCK,
+                 PinName NCS,
+                 PinName NIRQ)
+        : _trigger(trigger),
+        _echo0(echo0),
+        _echo1(echo1),
+        _echo2(echo2),
+        _echo3(echo3),
+        _echo4(echo4),
+        _echo5(echo5),
+        _rf(SDI,SDO,SCK,NCS,NIRQ) {
+
+
+    // initialises codes
+    _code[0] = CODE0;
+    _code[1] = CODE1;
+    _code[2] = CODE2;
+
+    //set callback execute to true
+    ValidPulse = true;
+
+    // Attach interrupts
+    _echo0.rise(this, &RFSRF05::_rising);
+    _echo0.fall(this, &RFSRF05::_falling);
+    _echo1.fall(this, &RFSRF05::_falling);
+    _echo2.fall(this, &RFSRF05::_falling);
+    _echo3.fall(this, &RFSRF05::_falling);
+    _echo4.fall(this, &RFSRF05::_falling);
+    _echo5.fall(this, &RFSRF05::_falling);
+
+    //init callabck function
+    callbackfunc = NULL;
+    callbackobj = NULL;
+    mcallbackfunc = NULL;
+
+    // innitialises beacon counter
+    _beacon_counter = 0;
+
+    //Interrupts every 50ms
+    _ticker.attach(this, &RFSRF05::_startRange, 0.05);
+}
+
+void RFSRF05::_startRange() {
+
+    // increments counter
+    _beacon_counter = (_beacon_counter + 1) % 3;
+
+    // writes code to RF port
+    _rf.write(_code[_beacon_counter]);
+
+    // send a trigger pulse, 10uS long
+    ValidPulse = false;
+    
+    _trigger = 1;
+    wait_us (10);
+    _trigger = 0;
+}
+
+// Clear and start the timer at the begining of the echo pulse
+void RFSRF05::_rising(void) {
+
+    _timer.reset();
+    _timer.start();
+    
+    //Set callback execute to ture
+    ValidPulse = true;
+}
+
+// Stop and read the timer at the end of the pulse
+void RFSRF05::_falling(void) {
+    _timer.stop();
+
+    if (ValidPulse) {
+        ValidPulse = false;
+    
+        //Calucate distance
+        _dist[_beacon_counter] =  _timer.read_us()/29.0;
+    
+        if (callbackfunc)
+            (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
+            
+        if (callbackobj && mcallbackfunc)
+            (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter]);
+
+    }
+    
+}
+
+float RFSRF05::read0() {
+    // returns distance
+    return (_dist[0]);
+}
+
+float RFSRF05::read1() {
+    // returns distance
+    return (_dist[1]);
+}
+
+float RFSRF05::read2() {
+    // returns distance
+    return (_dist[2]);
+}
+
+//SRF05::operator float() {
+//    return read();
+//}
diff -r 000000000000 -r a0285293f6a6 RFSRF05/RFSRF05.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFSRF05/RFSRF05.h	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,108 @@
+/* mbed SRF05 Ultrasonic Rangefiner Library
+ * Copyright (c) 2007-2010, cstyles, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+
+#ifndef MBED_RFSRF05_H
+#define MBED_RFSRF05_H
+
+#include "mbed.h"
+#include "RF12B.h"
+
+#define CODE0 0x22
+#define CODE1 0x44
+#define CODE2 0x88
+
+/* SAMPLE IMPLEMENTATION!
+RFSRF05 my_srf(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9);
+
+
+void callbinmain(int num, float dist) {
+    //Here is where you deal with your brand new reading ;D
+}
+
+int main() {
+    pc.printf("Hello World of RobotSonar!\r\n");
+    my_srf.callbackfunc = callbinmain;
+    
+    while (1);
+}
+
+ */
+class DummyCT;
+ 
+class RFSRF05 {
+public:
+
+    RFSRF05(
+    PinName trigger, 
+    PinName echo0,
+    PinName echo1,
+    PinName echo2,
+    PinName echo3,
+    PinName echo4,
+    PinName echo5,
+    PinName SDI,
+    PinName SDO,
+    PinName SCK,
+    PinName NCS,
+    PinName NIRQ);
+    
+    /** A non-blocking function that will return the last measurement
+     *
+     * @returns floating point representation of distance in cm
+     */
+    float read0();
+    float read1();
+    float read2();
+    
+    /** A assigns a callback function when a new reading is available **/
+    void (*callbackfunc)(int beaconnum, float distance);
+    DummyCT* callbackobj;
+    void (DummyCT::*mcallbackfunc)(int beaconnum, float distance);
+    
+
+
+    /** A short hand way of using the read function */
+    operator float();
+    
+private :
+    RF12B _rf;
+    DigitalOut _trigger;
+    InterruptIn _echo0;
+    InterruptIn _echo1;
+    InterruptIn _echo2;
+    InterruptIn _echo3;
+    InterruptIn _echo4;
+    InterruptIn _echo5;
+    Timer _timer;
+    Ticker _ticker;
+    void _rising (void);
+    void _falling (void);
+    void _startRange (void);
+    float _dist[3];
+    char _code[3];
+    int _beacon_counter;
+    bool ValidPulse;
+    
+};
+
+#endif
diff -r 000000000000 -r a0285293f6a6 RFSRF05/RFSerial.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFSRF05/RFSerial.cpp	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,25 @@
+
+/* Constructor */
+#include "RFSerial.h"
+
+RFSerial::RFSerial(PinName _SDI,
+                   PinName _SDO,
+                   PinName _SCK,
+                   PinName _NCS,
+                   PinName _NIRQ)
+:RF12B(_SDI, _SDO, _SCK, _NCS, _NIRQ) {
+    
+}
+
+// Stream implementation functions
+int RFSerial::_putc(int value) {
+    RF12B::write((unsigned char) value);
+    return value;
+}
+int RFSerial::_getc() {
+    if(available()) {
+        return RF12B::read();
+    } else {
+        return EOF;
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r a0285293f6a6 RFSRF05/RFSerial.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFSRF05/RFSerial.h	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,17 @@
+#include "mbed.h"
+#include "RF12B.h"
+
+class RFSerial : public Stream, public RF12B {
+public:
+    /* Constructor */
+    RFSerial(PinName _SDI,
+          PinName _SDO,
+          PinName _SCK,
+          PinName _NCS,
+          PinName _NIRQ);
+     
+protected:
+    // Stream implementation functions
+    virtual int _putc(int value);
+    virtual int _getc();    
+};
\ No newline at end of file
diff -r 000000000000 -r a0285293f6a6 RFSRF05/RF_defs.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFSRF05/RF_defs.h	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,478 @@
+/*
+ *  Open HR20
+ *
+ *  target:     ATmega169 @ 4 MHz in Honnywell Rondostat HR20E
+ *
+ *  compiler:   WinAVR-20071221
+ *              avr-libc 1.6.0
+ *              GCC 4.2.2
+ *
+ *  copyright:  2008 Dario Carluccio (hr20-at-carluccio-dot-de)
+ *              2008 Jiri Dobry (jdobry-at-centrum-dot-cz)
+ *              2008 Mario Fischer (MarioFischer-at-gmx-dot-net)
+ *              2007 Michael Smola (Michael-dot-Smola-at-gmx-dot-net)
+ *
+ *  license:    This program is free software; you can redistribute it and/or
+ *              modify it under the terms of the GNU Library General Public
+ *              License as published by the Free Software Foundation; either
+ *              version 2 of the License, or (at your option) any later version.
+ *
+ *              This program 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
+ */
+
+/*
+ * \file       rfm.h
+ * \brief      functions to control the RFM12 Radio Transceiver Module
+ * \author     Mario Fischer <MarioFischer-at-gmx-dot-net>; Michael Smola <Michael-dot-Smola-at-gmx-dot-net>
+ * \date       $Date: 2010/04/17 17:57:02 $
+ * $Rev: 260 $
+ */
+
+
+//#pragma once // multi-iclude prevention. gcc knows this pragma
+#ifndef rfm_H
+#define rfm_H
+
+
+#define RFM_SPI_16(OUTVAL)            rfm_spi16(OUTVAL) //<! a function that gets a uint16_t (clocked out value) and returns a uint16_t (clocked in value)
+
+#define RFM_CLK_OUTPUT 0
+
+/*
+#define RFM_TESTPIN_INIT
+#define RFM_TESTPIN_ON
+#define RFM_TESTPIN_OFF
+#define RFM_TESTPIN_TOG
+
+#define RFM_CONFIG_DISABLE            0x00 //<! RFM_CONFIG_*** are combinable flags, what the RFM shold do
+#define RFM_CONFIG_BROADCASTSTATUS    0x01 //<! Flag that enables the HR20's status broadcast every minute
+
+#define RFM_CONFIG_ENABLEALL        0xff
+*/
+
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// RFM status bits
+//
+///////////////////////////////////////////////////////////////////////////////
+
+// Interrupt bits, latched ////////////////////////////////////////////////////
+
+#define RFM_STATUS_FFIT 0x8000 // RX FIFO reached the progr. number of bits
+                               // Cleared by any FIFO read method
+
+#define RFM_STATUS_RGIT 0x8000 // TX register is ready to receive
+                               // Cleared by TX write
+
+#define RFM_STATUS_POR  0x4000 // Power On reset
+                               // Cleared by read status
+
+#define RFM_STATUS_RGUR 0x2000 // TX register underrun, register over write
+                               // Cleared by read status
+
+#define RFM_STATUS_FFOV 0x2000 // RX FIFO overflow
+                               // Cleared by read status
+
+#define RFM_STATUS_WKUP 0x1000 // Wake up timer overflow
+                               // Cleared by read status
+
+#define RFM_STATUS_EXT  0x0800 // Interupt changed to low
+                               // Cleared by read status
+
+#define RFM_STATUS_LBD  0x0400 // Low battery detect
+
+// Status bits ////////////////////////////////////////////////////////////////
+
+#define RFM_STATUS_FFEM 0x0200 // FIFO is empty
+#define RFM_STATUS_ATS  0x0100 // TX mode: Strong enough RF signal
+#define RFM_STATUS_RSSI 0x0100 // RX mode: signal strength above programmed limit
+#define RFM_STATUS_DQD  0x0080 // Data Quality detector output
+#define RFM_STATUS_CRL  0x0040 // Clock recovery lock
+#define RFM_STATUS_ATGL 0x0020 // Toggling in each AFC cycle
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 1. Configuration Setting Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_CONFIG               0x8000
+
+#define RFM_CONFIG_EL            0x8080 // Enable TX Register
+#define RFM_CONFIG_EF            0x8040 // Enable RX FIFO buffer
+#define RFM_CONFIG_BAND_315      0x8000 // Frequency band
+#define RFM_CONFIG_BAND_433      0x8010
+#define RFM_CONFIG_BAND_868      0x8020
+#define RFM_CONFIG_BAND_915      0x8030
+#define RFM_CONFIG_X_8_5pf       0x8000 // Crystal Load Capacitor
+#define RFM_CONFIG_X_9_0pf       0x8001
+#define RFM_CONFIG_X_9_5pf       0x8002
+#define RFM_CONFIG_X_10_0pf      0x8003
+#define RFM_CONFIG_X_10_5pf      0x8004
+#define RFM_CONFIG_X_11_0pf      0x8005
+#define RFM_CONFIG_X_11_5pf      0x8006
+#define RFM_CONFIG_X_12_0pf      0x8007
+#define RFM_CONFIG_X_12_5pf      0x8008
+#define RFM_CONFIG_X_13_0pf      0x8009
+#define RFM_CONFIG_X_13_5pf      0x800A
+#define RFM_CONFIG_X_14_0pf      0x800B
+#define RFM_CONFIG_X_14_5pf      0x800C
+#define RFM_CONFIG_X_15_0pf      0x800D
+#define RFM_CONFIG_X_15_5pf      0x800E
+#define RFM_CONFIG_X_16_0pf      0x800F
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 2. Power Management Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_POWER_MANAGEMENT     0x8200
+
+#define RFM_POWER_MANAGEMENT_ER  0x8280 // Enable receiver
+#define RFM_POWER_MANAGEMENT_EBB 0x8240 // Enable base band block
+#define RFM_POWER_MANAGEMENT_ET  0x8220 // Enable transmitter
+#define RFM_POWER_MANAGEMENT_ES  0x8210 // Enable synthesizer
+#define RFM_POWER_MANAGEMENT_EX  0x8208 // Enable crystal oscillator
+#define RFM_POWER_MANAGEMENT_EB  0x8204 // Enable low battery detector
+#define RFM_POWER_MANAGEMENT_EW  0x8202 // Enable wake-up timer
+#define RFM_POWER_MANAGEMENT_DC  0x8201 // Disable clock output of CLK pin
+
+#ifndef RFM_CLK_OUTPUT
+    #error RFM_CLK_OUTPUT must be defined to 0 or 1
+#endif
+#if RFM_CLK_OUTPUT
+    #define RFM_TX_ON_PRE() RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_TX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_ET | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_RX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_ER | \
+                                RFM_POWER_MANAGEMENT_EBB | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_OFF()       RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_EX )
+#else
+    #define RFM_TX_ON_PRE() RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_DC | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_TX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_DC | \
+                                RFM_POWER_MANAGEMENT_ET | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_RX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_DC  | \
+                                RFM_POWER_MANAGEMENT_ER | \
+                                RFM_POWER_MANAGEMENT_EBB | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_OFF()       RFM_SPI_16(RFM_POWER_MANAGEMENT_DC)
+#endif
+///////////////////////////////////////////////////////////////////////////////
+//
+// 3. Frequency Setting Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_FREQUENCY            0xA000
+
+#define RFM_FREQ_315Band(v) (uint16_t)((v/10.0-31)*4000)
+#define RFM_FREQ_433Band(v) (uint16_t)((v/10.0-43)*4000)
+#define RFM_FREQ_868Band(v) (uint16_t)((v/20.0-43)*4000)
+#define RFM_FREQ_915Band(v) (uint16_t)((v/30.0-30)*4000)
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 4. Data Rate Command
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#define RFM_BAUD_RATE            9600
+
+#define RFM_DATA_RATE            0xC600
+
+#define RFM_DATA_RATE_CS         0xC680
+#define RFM_DATA_RATE_4800       0xC647
+#define RFM_DATA_RATE_9600       0xC623
+#define RFM_DATA_RATE_19200      0xC611
+#define RFM_DATA_RATE_38400      0xC608
+#define RFM_DATA_RATE_57600      0xC605
+
+#define RFM_SET_DATARATE(baud)        ( ((baud)<5400) ? (RFM_DATA_RATE_CS|((43104/(baud))-1)) : (RFM_DATA_RATE|((344828UL/(baud))-1)) )
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 5. Receiver Control Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_RX_CONTROL           0x9000
+
+#define RFM_RX_CONTROL_P20_INT   0x9000 // Pin20 = ExternalInt
+#define RFM_RX_CONTROL_P20_VDI   0x9400 // Pin20 = VDI out
+
+#define RFM_RX_CONTROL_VDI_FAST  0x9000 // fast   VDI Response time
+#define RFM_RX_CONTROL_VDI_MED   0x9100 // medium
+#define RFM_RX_CONTROL_VDI_SLOW  0x9200 // slow
+#define RFM_RX_CONTROL_VDI_ON    0x9300 // Always on
+
+#define RFM_RX_CONTROL_BW_400    0x9020 // bandwidth 400kHz
+#define RFM_RX_CONTROL_BW_340    0x9040 // bandwidth 340kHz
+#define RFM_RX_CONTROL_BW_270    0x9060 // bandwidth 270kHz
+#define RFM_RX_CONTROL_BW_200    0x9080 // bandwidth 200kHz
+#define RFM_RX_CONTROL_BW_134    0x90A0 // bandwidth 134kHz
+#define RFM_RX_CONTROL_BW_67     0x90C0 // bandwidth 67kHz
+
+#define RFM_RX_CONTROL_GAIN_0    0x9000 // LNA gain  0db
+#define RFM_RX_CONTROL_GAIN_6    0x9008 // LNA gain -6db
+#define RFM_RX_CONTROL_GAIN_14   0x9010 // LNA gain -14db
+#define RFM_RX_CONTROL_GAIN_20   0x9018 // LNA gain -20db
+
+#define RFM_RX_CONTROL_RSSI_103  0x9000 // DRSSI threshold -103dbm
+#define RFM_RX_CONTROL_RSSI_97   0x9001 // DRSSI threshold -97dbm
+#define RFM_RX_CONTROL_RSSI_91   0x9002 // DRSSI threshold -91dbm
+#define RFM_RX_CONTROL_RSSI_85   0x9003 // DRSSI threshold -85dbm
+#define RFM_RX_CONTROL_RSSI_79   0x9004 // DRSSI threshold -79dbm
+#define RFM_RX_CONTROL_RSSI_73   0x9005 // DRSSI threshold -73dbm
+//#define RFM_RX_CONTROL_RSSI_67   0x9006 // DRSSI threshold -67dbm // RF12B reserved
+//#define RFM_RX_CONTROL_RSSI_61   0x9007 // DRSSI threshold -61dbm // RF12B reserved
+
+#define RFM_RX_CONTROL_BW(baud)        (((baud)<8000) ? \
+                                    RFM_RX_CONTROL_BW_67 : \
+                                    ( \
+                                        ((baud)<30000) ? \
+                                        RFM_RX_CONTROL_BW_134 : \
+                                        RFM_RX_CONTROL_BW_200 \
+                                    ))
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 6. Data Filter Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_DATA_FILTER          0xC228
+
+#define RFM_DATA_FILTER_AL       0xC2A8 // clock recovery auto-lock
+#define RFM_DATA_FILTER_ML       0xC268 // clock recovery fast mode
+#define RFM_DATA_FILTER_DIG      0xC228 // data filter type digital
+#define RFM_DATA_FILTER_ANALOG   0xC238 // data filter type analog
+#define RFM_DATA_FILTER_DQD(level) (RFM_DATA_FILTER | (level & 0x7))
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 7. FIFO and Reset Mode Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_FIFO                 0xCA00
+
+#define RFM_FIFO_AL              0xCA04 // FIFO Start condition sync-word/always
+#define RFM_FIFO_FF              0xCA02 // Enable FIFO fill
+#define RFM_FIFO_DR              0xCA01 // Disable hi sens reset mode
+#define RFM_FIFO_IT(level)       (RFM_FIFO | (( (level) & 0xF)<<4))
+
+#define RFM_FIFO_OFF()            RFM_SPI_16(RFM_FIFO_IT(8) |               RFM_FIFO_DR)
+#define RFM_FIFO_ON()             RFM_SPI_16(RFM_FIFO_IT(8) | RFM_FIFO_FF | RFM_FIFO_DR)
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// 8. Receiver FIFO Read
+//
+/////////////////////////////////////////////////////////////////////////////
+
+#define RFM_READ_FIFO()           (RFM_SPI_16(0xB000) & 0xFF)
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// 9. AFC Command
+//
+/////////////////////////////////////////////////////////////////////////////
+
+#define RFM_AFC                  0xC400
+
+#define RFM_AFC_EN               0xC401
+#define RFM_AFC_OE               0xC402
+#define RFM_AFC_FI               0xC404
+#define RFM_AFC_ST               0xC408
+
+// Limits the value of the frequency offset register to the next values:
+
+#define RFM_AFC_RANGE_LIMIT_NO    0xC400 // 0: No restriction
+#define RFM_AFC_RANGE_LIMIT_15_16 0xC410 // 1: +15 fres to -16 fres
+#define RFM_AFC_RANGE_LIMIT_7_8   0xC420 // 2: +7 fres to -8 fres
+#define RFM_AFC_RANGE_LIMIT_3_4   0xC430 // 3: +3 fres to -4 fres
+
+// fres=2.5 kHz in 315MHz and 433MHz Bands
+// fres=5.0 kHz in 868MHz Band
+// fres=7.5 kHz in 915MHz Band
+
+#define RFM_AFC_AUTO_OFF         0xC400 // 0: Auto mode off (Strobe is controlled by microcontroller)
+#define RFM_AFC_AUTO_ONCE        0xC440 // 1: Runs only once after each power-up
+#define RFM_AFC_AUTO_VDI         0xC480 // 2: Keep the foffset only during receiving(VDI=high)
+#define RFM_AFC_AUTO_INDEPENDENT 0xC4C0 // 3: Keep the foffset value independently trom the state of the VDI signal
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 10. TX Configuration Control Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_TX_CONTROL           0x9800
+
+#define RFM_TX_CONTROL_POW_0     0x9800
+#define RFM_TX_CONTROL_POW_3     0x9801
+#define RFM_TX_CONTROL_POW_6     0x9802
+#define RFM_TX_CONTROL_POW_9     0x9803
+#define RFM_TX_CONTROL_POW_12    0x9804
+#define RFM_TX_CONTROL_POW_15    0x9805
+#define RFM_TX_CONTROL_POW_18    0x9806
+#define RFM_TX_CONTROL_POW_21    0x9807
+#define RFM_TX_CONTROL_MOD_15    0x9800
+#define RFM_TX_CONTROL_MOD_30    0x9810
+#define RFM_TX_CONTROL_MOD_45    0x9820
+#define RFM_TX_CONTROL_MOD_60    0x9830
+#define RFM_TX_CONTROL_MOD_75    0x9840
+#define RFM_TX_CONTROL_MOD_90    0x9850
+#define RFM_TX_CONTROL_MOD_105   0x9860
+#define RFM_TX_CONTROL_MOD_120   0x9870
+#define RFM_TX_CONTROL_MOD_135   0x9880
+#define RFM_TX_CONTROL_MOD_150   0x9890
+#define RFM_TX_CONTROL_MOD_165   0x98A0
+#define RFM_TX_CONTROL_MOD_180   0x98B0
+#define RFM_TX_CONTROL_MOD_195   0x98C0
+#define RFM_TX_CONTROL_MOD_210   0x98D0
+#define RFM_TX_CONTROL_MOD_225   0x98E0
+#define RFM_TX_CONTROL_MOD_240   0x98F0
+#define RFM_TX_CONTROL_MP        0x9900
+
+#define RFM_TX_CONTROL_MOD(baud)    (((baud)<8000) ? \
+                                    RFM_TX_CONTROL_MOD_45 : \
+                                    ( \
+                                        ((baud)<20000) ? \
+                                        RFM_TX_CONTROL_MOD_60 : \
+                                        ( \
+                                            ((baud)<30000) ? \
+                                            RFM_TX_CONTROL_MOD_75 : \
+                                            ( \
+                                                ((baud)<40000) ? \
+                                                RFM_TX_CONTROL_MOD_90 : \
+                                                RFM_TX_CONTROL_MOD_120 \
+                                            ) \
+                                        ) \
+                                    ))
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// 11. Transmitter Register Write Command
+//
+/////////////////////////////////////////////////////////////////////////////
+
+//#define RFM_WRITE(byte)  RFM_SPI_16(0xB800 | ((byte) & 0xFF))
+#define RFM_WRITE(byte)  RFM_SPI_16(0xB800 | (byte) )
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 12. Wake-up Timer Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_WAKEUP_TIMER         0xE000
+#define RFM_WAKEUP_SET(time)     RFM_SPI_16(RFM_WAKEUP_TIMER | (time))
+
+#define RFM_WAKEUP_480s          (RFM_WAKEUP_TIMER |(11 << 8)| 234)
+#define RFM_WAKEUP_240s          (RFM_WAKEUP_TIMER |(10 << 8)| 234)
+#define RFM_WAKEUP_120s          (RFM_WAKEUP_TIMER |(9  << 8)| 234)
+#define RFM_WAKEUP_119s          (RFM_WAKEUP_TIMER |(9  << 8)| 232)
+
+#define RFM_WAKEUP_60s           (RFM_WAKEUP_TIMER |(8 << 8) | 235)
+#define RFM_WAKEUP_59s           (RFM_WAKEUP_TIMER |(8 << 8) | 230)
+
+#define RFM_WAKEUP_30s           (RFM_WAKEUP_TIMER |(7 << 8) | 235)
+#define RFM_WAKEUP_29s           (RFM_WAKEUP_TIMER |(7 << 8) | 227)
+
+#define RFM_WAKEUP_8s            (RFM_WAKEUP_TIMER |(5 << 8) | 250)
+#define RFM_WAKEUP_7s            (RFM_WAKEUP_TIMER |(5 << 8) | 219)
+#define RFM_WAKEUP_6s            (RFM_WAKEUP_TIMER |(6 << 8) |  94)
+#define RFM_WAKEUP_5s            (RFM_WAKEUP_TIMER |(5 << 8) | 156)
+#define RFM_WAKEUP_4s            (RFM_WAKEUP_TIMER |(5 << 8) | 125)
+#define RFM_WAKEUP_1s            (RFM_WAKEUP_TIMER |(2 << 8) | 250)
+#define RFM_WAKEUP_900ms         (RFM_WAKEUP_TIMER |(2 << 8) | 225)
+#define RFM_WAKEUP_800ms         (RFM_WAKEUP_TIMER |(2 << 8) | 200)
+#define RFM_WAKEUP_700ms         (RFM_WAKEUP_TIMER |(2 << 8) | 175)
+#define RFM_WAKEUP_600ms         (RFM_WAKEUP_TIMER |(2 << 8) | 150)
+#define RFM_WAKEUP_500ms         (RFM_WAKEUP_TIMER |(2 << 8) | 125)
+#define RFM_WAKEUP_400ms         (RFM_WAKEUP_TIMER |(2 << 8) | 100)
+#define RFM_WAKEUP_300ms         (RFM_WAKEUP_TIMER |(2 << 8) |  75)
+#define RFM_WAKEUP_200ms         (RFM_WAKEUP_TIMER |(2 << 8) |  50)
+#define RFM_WAKEUP_100ms         (RFM_WAKEUP_TIMER |(2 << 8) |  25)
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 13. Low Duty-Cycle Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_LOW_DUTY_CYCLE       0xC800
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 14. Low Battery Detector Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_LOW_BATT_DETECT           0xC000
+#define RFM_LOW_BATT_DETECT_D_1MHZ    0xC000
+#define RFM_LOW_BATT_DETECT_D_1_25MHZ 0xC020
+#define RFM_LOW_BATT_DETECT_D_1_66MHZ 0xC040
+#define RFM_LOW_BATT_DETECT_D_2MHZ    0xC060
+#define RFM_LOW_BATT_DETECT_D_2_5MHZ  0xC080
+#define RFM_LOW_BATT_DETECT_D_3_33MHZ 0xC0A0
+#define RFM_LOW_BATT_DETECT_D_5MHZ    0xC0C0
+#define RFM_LOW_BATT_DETECT_D_10MHZ   0xC0E0
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 15. Status Read Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_READ_STATUS()       RFM_SPI_16(0x0000)
+#define RFM_READ_STATUS_FFIT()  SPI_1 (0x00)
+#define RFM_READ_STATUS_RGIT    RFM_READ_STATUS_FFIT
+
+///////////////////////////////////////////////////////////////////////////////
+
+// RFM air protocol flags:
+
+#define RFMPROTO_FLAGS_BITASK_PACKETTYPE        0b11000000 //!< the uppermost 2 bits of the flags field encode the packettype
+#define RFMPROTO_FLAGS_PACKETTYPE_BROADCAST        0b00000000 //!< broadcast packettype (message from hr20, protocol; step 1)
+#define RFMPROTO_FLAGS_PACKETTYPE_COMMAND        0b01000000 //!< command packettype (message to hr20, protocol; step 2)
+#define RFMPROTO_FLAGS_PACKETTYPE_REPLY            0b10000000 //!< reply packettype (message from hr20, protocol; step 3)
+#define RFMPROTO_FLAGS_PACKETTYPE_SPECIAL        0b11000000 //!< currently unused packettype
+
+#define RFMPROTO_FLAGS_BITASK_DEVICETYPE        0b00011111 //!< the lowermost 5 bytes denote the device type. this way other sensors and actors may coexist
+#define RFMPROTO_FLAGS_DEVICETYPE_OPENHR20        0b00010100 //!< topen HR20 device type. 10100 is for decimal 20
+
+#define RFMPROTO_IS_PACKETTYPE_BROADCAST(FLAGS)    ( RFMPROTO_FLAGS_PACKETTYPE_BROADCAST == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_PACKETTYPE_COMMAND(FLAGS)    ( RFMPROTO_FLAGS_PACKETTYPE_COMMAND   == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_PACKETTYPE_REPLY(FLAGS)        ( RFMPROTO_FLAGS_PACKETTYPE_REPLY     == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_PACKETTYPE_SPECIAL(FLAGS)    ( RFMPROTO_FLAGS_PACKETTYPE_SPECIAL   == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_DEVICETYPE_OPENHR20(FLAGS)    ( RFMPROTO_FLAGS_DEVICETYPE_OPENHR20  == ((FLAGS) & RFMPROTO_FLAGS_BITASK_DEVICETYPE) )
+
+///////////////////////////////////////////////////////////////////////////////
+
+#endif
\ No newline at end of file