Oskar Weigl
Kalman filter for Eurobot
Revision 0:a0285293f6a6, committed 2012-03-20
- Comitter:
- madcowswe
- Date:
- Tue Mar 20 12:43:16 2012 +0000
- Commit message:
- Algo done, vars not done
Changed in this revision
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�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