Diff: Processes/Kalman/Kalman.cpp

Revision:

32:ada943ecaceb

Parent:

30:00e1493b44f0

Child:

36:f8e7f0a72a3d

--- a/Processes/Kalman/Kalman.cpp	Wed Apr 10 14:34:07 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Wed Apr 10 18:04:47 2013 +0000
@@ -29,6 +29,7 @@
 Mutex statelock;
 
 float RawReadings[maxmeasure+1];
+int sensorseenflags = 0;
 float IRphaseOffset;
 
 bool Kalman_inited = 0;
@@ -51,6 +52,9 @@
     
     //WARNING: HARDCODED! TODO: fix it so it works for both sides!
     
+    printf("waiting for all sonar, and at least 1 IR\r\n");
+    while( ((sensorseenflags & 0x7)^0x7) || !(sensorseenflags & 0x7<<3) );
+    
     //solve for our position (assume perfect bias)
     const float d = beaconpos[2].y - beaconpos[1].y;
     const float i = beaconpos[2].y - beaconpos[0].y;
@@ -77,20 +81,36 @@
     IRMeasuresloc[2] = RawReadings[IR2];
     printf("IR 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
 
-    float IR_Offsets[3];
-    float fromb0offset = 0;
+    float IR_Offsets[3] = {0};
+    float frombrefoffset = 0;
+    int refbeacon = 0;
+    
+    for (int i = 0; i < 3; i++){
+        if (sensorseenflags & 1<<(3+i)){
+            refbeacon = i;
+            break;
+        }
+    }
+    
+    printf("refbeacon is %d\r\n", refbeacon);
+    
+    int cnt = 0;
     for (int i = 0; i < 3; i++) {
 
-        //Compute IR offset
-        float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
-
-        //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
-        IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
-
-        fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
+        if (sensorseenflags & 1<<(3+i)){
+            cnt++;
+            
+            //Compute IR offset
+            float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
+    
+            //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
+            IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
+    
+            frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]);
+        }
     }
 
-    IRphaseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
+    IRphaseOffset = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt);
 
     //debug
     printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI);
@@ -220,6 +240,8 @@
 
 void runupdate(measurement_t type, float value, float variance)
 {
+    sensorseenflags |= 1<<type;
+
     if (!Kalman_inited) {
         RawReadings[type] = value;
     } else {
@@ -233,7 +255,7 @@
         measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
         if (measured) {
             measured->mtype = type;
-            measured->value = value;
+            measured->value = RawReadings[type];
             measured->variance = variance;
 
             osStatus putret = measureMQ.put(measured);
@@ -343,7 +365,7 @@
                 }
 
                 Matrix<float, 3, 1> PH (P * trans(H));
-                S = (H * PH)(0,0) + variance;
+                S = (H * PH)(0,0) + variance*10; //TODO: temp hack
 
                 if (aborton2stddev && Y*Y > 4 * S) {
                     statelock.unlock();