Kalman.h

00001 #ifndef KALMAN_H
00002 #define KALMAN_H
00003 
00004 #include "globals.h"
00005 
00006 
00007 #include "rtos.h"
00008 //#include "Matrix.h"
00009 #include "encoders.h"
00010 #include "RFSRF05.h"
00011 #include "IR.h"
00012 #include "ui.h"
00013 
00014 #include <tvmet/Matrix.h>
00015 #include <tvmet/Vector.h>
00016 using namespace tvmet;
00017 
00018 
00019 class Kalman {
00020 public:
00021     enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3};
00022     static const measurement_t maxmeasure = IR3;
00023 
00024     Kalman(Encoders &encodersin,
00025            UI &uiin,
00026            PinName Sonar_Trig,
00027            PinName Sonar_Echo0,
00028            PinName Sonar_Echo1,
00029            PinName Sonar_Echo2,
00030            PinName Sonar_Echo3,
00031            PinName Sonar_Echo4,
00032            PinName Sonar_Echo5,
00033            PinName Sonar_SDI,
00034            PinName Sonar_SDO,
00035            PinName Sonar_SCK,
00036            PinName Sonar_NCS,
00037            PinName Sonar_NIRQ);
00038 
00039     void predict();
00040     void runupdate(measurement_t type, float value, float variance);
00041 
00042     //State variables
00043     Vector<float, 3>  X;
00044     Matrix<float, 3, 3>  P;
00045     Mutex statelock;
00046 
00047     float SonarMeasures[3];
00048     float IRMeasures[3];
00049     float IR_Offset;
00050     float Sonar_Offset;
00051     Mutex InitLock;
00052 
00053     bool Kalman_init;
00054 
00055     //The IR is public so it's possible to print the offset in the print function
00056     IR ir;
00057 
00058     //Initialises the kalman filter
00059     void KalmanInit();
00060     
00061     // reset kalman states
00062     void KalmanReset();
00063 
00064 private:
00065 
00066     //Sensor interfaces
00067     RFSRF05 sonararray;
00068     Encoders& encoders;
00069     UI& ui;
00070 
00071     Thread predictthread;
00072     void predictloop();
00073     static void predictloopwrapper(void const *argument) {
00074         ((Kalman*)argument)->predictloop();
00075     }
00076     RtosTimer predictticker;
00077 
00078 //    Thread sonarthread;
00079 //    void sonarloop();
00080 //    static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); }
00081 //    RtosTimer sonarticker;
00082 
00083     struct measurmentdata {
00084         measurement_t mtype;
00085         float value;
00086         float variance;
00087     } ;
00088 
00089     Mail <measurmentdata, 16> measureMQ;
00090 
00091     Thread updatethread;
00092     void updateloop();
00093     static void updateloopwrapper(void const *argument) {
00094         ((Kalman*)argument)->updateloop();
00095     }
00096 
00097 
00098 };
00099 
00100 #endif //KALMAN_H