2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Kalman/Kalman.cpp
- Revision:
- 31:ada943ecaceb
- Parent:
- 29:00e1493b44f0
- Child:
- 35:f8e7f0a72a3d
diff -r 00e1493b44f0 -r ada943ecaceb Processes/Kalman/Kalman.cpp --- 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();