Oskar Weigl
/
Eurobot2013
We are going to win! wohoo
Kalman/Kalman.h
- Committer:
- sv
- Date:
- 2012-11-07
- Revision:
- 1:6799c07fe510
- Child:
- 5:a229f40c1210
File content as of revision 1:6799c07fe510:
#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); 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 IR_Offset; float Sonar_Offset; Mutex InitLock; 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(); // reset kalman states void KalmanReset(); 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