Kalman.h
- Committer:
- narshu
- Date:
- 2012-04-26
- Revision:
- 2:8aa491f77a0b
- Parent:
- 1:4964fa534202
File content as of revision 2:8aa491f77a0b:
#ifndef KALMAN_H #define KALMAN_H #include "globals.h" #include "rtos.h" //#include "Matrix.h" #include "motors.h" #include "RFSRF05.h" #include "IR.h" #include "ui.h" #include <tvmet/Matrix.h> #include <tvmet/Vector.h> using namespace tvmet; class Kalman { public: enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3}; static const measurement_t maxmeasure = IR3; Kalman(Motors &motorsin, UI &uiin, PinName Sonar_Trig, PinName Sonar_Echo0, PinName Sonar_Echo1, PinName Sonar_Echo2, PinName Sonar_Echo3, PinName Sonar_Echo4, PinName Sonar_Echo5, PinName Sonar_SDI, PinName Sonar_SDO, PinName Sonar_SCK, PinName Sonar_NCS, PinName Sonar_NIRQ, PinName IR_Tx, PinName IR_Rx); void predict(); void runupdate(measurement_t type, float value, float variance); //State variables Vector<float, 3> X; Matrix<float, 3, 3> P; Mutex statelock; float SonarMeasures[3]; float IRMeasures[3]; float SonarMeasure_Offset[3]; bool Kalman_init; //The IR is public so it's possible to print the offset in the print function IR ir; //Initialises the kalman filter void KalmanInit(); private: //Sensor interfaces RFSRF05 sonararray; Motors& motors; UI& ui; Thread predictthread; void predictloop(); static void predictloopwrapper(void const *argument) { ((Kalman*)argument)->predictloop(); } RtosTimer predictticker; // Thread sonarthread; // void sonarloop(); // static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); } // RtosTimer sonarticker; struct measurmentdata { measurement_t mtype; float value; float variance; } ; Mail <measurmentdata, 16> measureMQ; Thread updatethread; void updateloop(); static void updateloopwrapper(void const *argument) { ((Kalman*)argument)->updateloop(); } }; #endif //KALMAN_H