Diff: Kalman/Kalman.h

Revision:

1:6799c07fe510

Child:

5:a229f40c1210

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Kalman.h	Wed Nov 07 14:37:35 2012 +0000
@@ -0,0 +1,100 @@
+#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
\ No newline at end of file