Diff: Kalman/Kalman.cpp

Revision:

8:ffc7d8af2d5a

Parent:

7:f9c59a3e4155

--- a/Kalman/Kalman.cpp	Fri Apr 27 16:37:26 2012 +0000
+++ b/Kalman/Kalman.cpp	Fri Apr 27 18:36:54 2012 +0000
@@ -167,6 +167,9 @@
 
 void Kalman::predictloop() {
 
+    OLED4 = !ui.regid(0, 3);
+    OLED4 = !ui.regid(1, 9);
+
     float lastleft = 0;
     float lastright = 0;
 
@@ -234,12 +237,15 @@
             0, 0, varang + varangdt;
 
         P = F * P * trans(F) + Q;
+        
+        //Update UI
+        float statecpy[] = {X(0), X(1), X(2)};
+        OLED4 = !ui.updateval(0, statecpy, 3) || OLED4;
+        
+        float Pcpy[] = {P(0,0), P(0,1), P(0,2), P(1,0), P(1,1), P(1,2), P(2,0), P(2,1), P(2,2)};
+        OLED4 = !ui.updateval(1, Pcpy, 9) || OLED4;
 
         statelock.unlock();
-        //Thread::wait(PREDICTPERIOD);
-
-        //cout << "predict" << X << endl;
-        //cout << P << endl;
     }
 }