From 3ca8ba3d69de226ae8835bb29e45c7bcd35793fe Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 29 Mar 2017 16:39:47 -0700 Subject: Tori Poser Works! There's a ton of code cruft, and the algorithm is currently too slow. BUT I can track an object using only 1 lighthouse for tracking, at (I believe) an update rate of at least 7.5 HZ. By tracking, I know the position and orientation of the lighthouses relative to the tracked object, and I know the tracked object's location relative to the lighthouse. I don't have the orientation of the tracked object relative to the lighthouse yet, but that should be easy given the rest of the "knowns." --- src/poser_octavioradii.c | 198 +++++++++++++++++++++++++++++++++++++++++++++-- src/poser_turveytori.c | 174 +++++++++++++++++++++++++++++++++++++---- src/survive_data.c | 37 ++++++++- 3 files changed, 381 insertions(+), 28 deletions(-) diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 3893085..0d8674c 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -4,8 +4,12 @@ typedef struct { - int something; - //Stuff +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; + + int hitCount[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; } OctavioRadiiData; #include @@ -45,6 +49,7 @@ typedef struct Point normal; // unit vector indicating the normal for the sensor double theta; // "horizontal" angular measurement from lighthouse radians double phi; // "vertical" angular measurement from lighthouse in radians. + int id; } TrackedSensor; typedef struct @@ -382,10 +387,10 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a } - printf("\ni=%d\n", i); + printf(" i=%d ", i); } -void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +static void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) { FLT estimate[MAX_RADII]; @@ -394,6 +399,12 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj estimate[i] = 2.38; } + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("%d, ", obj->sensor[i].id); + //} + SensorAngles angles[MAX_RADII]; PointPair pairs[MAX_POINT_PAIRS]; @@ -423,22 +434,119 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj // we should now have an estimate of the radii. - for (int i = 0; i < obj->numSensors; i++) + //for (int i = 0; i < obj->numSensors; i++) + for (int i = 0; i < 1; i++) { printf("radius[%d]: %f\n", i, estimate[i]); } + // (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) return; } +static void QuickPose(SurviveObject *so) +{ + OctavioRadiiData * td = so->PoserData; + + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // //FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // //FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + int lh = 0; + //printf("%d[%d], ",i,td->hitCount[i][lh][0]); + + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if ((td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0)) + + + { + if (td->hitCount[i][lh][0] > 10 && td->hitCount[i][lh][1] > 10) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; + + + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + + sensorCount++; + } + } + } + //printf("\n"); + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + FLT pos[3]; + FLT orient[4]; + SolveForLighthouseRadii(pos, orient, to); + } + + + } + + + free(to); + +} + + int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) { PoserType pt = pd->pt; SurviveContext * ctx = so->ctx; OctavioRadiiData * dd = so->PoserData; - if( !dd ) so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + if( !dd ) + { + so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + memset(dd, 0, sizeof(OctavioRadiiData)); + } + switch( pt ) { @@ -451,9 +559,81 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)pd; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; + + //printf("%d ", l->sensor_id); + + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); - break; - } + if ((dd->lastAxis[l->lh] != (l->acode & 0x1)) ) + { + int lastAxis = dd->lastAxis[l->lh]; + //printf("\n"); + //if (0 == l->lh) + // printf("or[%d,%d] ", l->lh,lastAxis); + + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int index = dd->angleIndex[l->lh][axis]; + if (dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] != 0) + { + //if (0 == l->lh) + // printf("%d ", i); + + dd->hitCount[i][l->lh][axis]++; + } + else + { + dd->hitCount[i][l->lh][axis] *= 0.5; + } + } + //if (0 == l->lh) + // printf("\n"); + //int foo = l->acode & 0x1; + //printf("%d", foo); + + + //if (axis) + { + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so); + } + // axis changed, time to increment the circular buffer index. + + + dd->angleIndex[l->lh][axis]++; + dd->angleIndex[l->lh][axis] = dd->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] = 0; + } + + } + dd->lastAxis[l->lh] = axis; + } + + //if (0 == l->lh) + // printf("(%d) ", l->sensor_id); + + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + dd->oldAngles[l->sensor_id][axis][l->lh][dd->angleIndex[l->lh][axis]] = l->angle; + break; } case POSERDATA_FULL_SCENE: { TrackedObject *to; @@ -482,6 +662,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; sensorCount++; } } @@ -509,6 +690,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; sensorCount++; } } diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 8b92860..22098d0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -74,6 +74,11 @@ typedef struct FLT down[3]; // populated by the IMU for posing int something; //Stuff + +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; } ToriData; @@ -526,7 +531,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } - printf("\ni=%d\n", i); + printf(" i=%d ", i); return lastPoint; } @@ -845,12 +850,12 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], rotation[3]}; FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + printf("{%8.8f, %8.8f, %8.8f} ", objPoint[0], objPoint[1], objPoint[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -873,7 +878,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. - for (FLT g = 0.1; g > 0.000000001; g *= 0.99) + for (FLT g = 0.1; g > 0.000000001 || i > 10000; g *= 0.99) { i++; FLT point1[4]; @@ -947,7 +952,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - printf("\nRi=%d\n", i); + printf(" Ri=%d ", i); } static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) { @@ -956,7 +961,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); quatrotatevector(objPoint, rotation, objPoint); - printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } @@ -1056,7 +1061,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in } - printf("\nRi=%d Fitness=%3f\n", i, lastMatchFitness); + printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1088,8 +1093,23 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } -Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) { + //printf("Solving for Lighthouse\n"); + + //printf("obj->numSensors = %d;\n", obj->numSensors); + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("obj->sensor[%d].normal.x = %f;\n", i, obj->sensor[i].normal.x); + // printf("obj->sensor[%d].normal.y = %f;\n", i, obj->sensor[i].normal.y); + // printf("obj->sensor[%d].normal.z = %f;\n", i, obj->sensor[i].normal.z); + // printf("obj->sensor[%d].point.x = %f;\n", i, obj->sensor[i].point.x); + // printf("obj->sensor[%d].point.y = %f;\n", i, obj->sensor[i].point.y); + // printf("obj->sensor[%d].point.z = %f;\n", i, obj->sensor[i].point.z); + // printf("obj->sensor[%d].phi = %f;\n", i, obj->sensor[i].phi); + // printf("obj->sensor[%d].theta = %f;\n\n", i, obj->sensor[i].theta); + //} PointsAndAngle pna[MAX_POINT_PAIRS]; volatile size_t sizeNeeded = sizeof(pna); @@ -1168,13 +1188,17 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); - FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); - printf("Distance is %f, Fitness is %f\n", distance, fitGd); + printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - FLT rot[4]; - SolveForRotation(rot, obj, refinedEstimateGd); + if (fitGd > 5) + { + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); + //printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); + } if (logFile) { updateHeader(logFile); @@ -1191,7 +1215,82 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static void QuickPose(SurviveObject *so) +{ + ToriData * td = so->PoserData; + + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + int lh = 0; + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + + sensorCount++; + } + } + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + SolveForLighthouse(to, 0); + printf("!\n"); + } + + + } + + + free(to); + +} @@ -1229,14 +1328,47 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)poserData; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); - if (0 == l->lh) + if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { - if (l->acode & 0x1) + int foo = l->acode & 0x1; + //printf("%d", foo); + + + //if (axis) { - printf("%2d: %8f\n", l->sensor_id, l->angle); + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so); + } + // axis changed, time to increment the circular buffer index. + td->angleIndex[l->lh][axis]++; + td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; + } + } + td->lastAxis[l->lh] = axis; } + + td->oldAngles[l->sensor_id][axis][l->lh][td->angleIndex[l->lh][axis]] = l->angle; break; } case POSERDATA_FULL_SCENE: @@ -1278,12 +1410,20 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + sensorCount++; } } to->numSensors = sensorCount; + SolveForLighthouse(to, 0); } { diff --git a/src/survive_data.c b/src/survive_data.c index 3eb5890..3f16c7c 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -79,10 +79,40 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) } } + int allZero = 1; + for (int q=0; q< 32; q++) + if (lcd->sweep.sweep_len[q] != 0) + allZero=0; + //if (!allZero) + // printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5, lcd->per_sweep.activeLighthouse); for (int i = 0; i < SENSORS_PER_OBJECT; i++) { + { + static int counts[SENSORS_PER_OBJECT][2] = {0}; + + if (lcd->per_sweep.activeLighthouse == 0 && !allZero) + { + if (lcd->sweep.sweep_len[i] != 0) + { + //printf("%d ", i); + //counts[i][lcd->per_sweep.activeAcode & 1] ++; + } + else + { + counts[i][lcd->per_sweep.activeAcode & 1] =0; + } + + //if (counts[i][0] > 10 && counts[i][1] > 10) + //{ + //printf("%d(%d,%d), ", i, counts[i][0], counts[i][1]); + //} + } + } + + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { + int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) @@ -94,6 +124,9 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) } } } + //if (!allZero) + // printf(" ..:..\n"); + } // clear out sweep data (could probably limit this to only after a "first" sync. // this is slightly more robust, so doing it here for now. @@ -108,6 +141,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) //static unsigned int recent_sync_count = -1; //static unsigned int activeSweepStartTime; + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); // Process any sweep data we have handle_lightcap2_process_sweep_data(so); @@ -126,7 +160,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; @@ -148,8 +181,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); - if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcd->per_sweep.activeLighthouse != -1) -- cgit v1.2.3