+ curSample = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
+
+ // find first high sample
+ if (!startFound && curSample > T55xx_READ_UPPER_THRESHOLD) {
+ if (curSample > firstSample) firstSample = curSample;
+ highFound = true;
+ } else if (!highFound) {
+ continue;
+ }
+
+ // skip until samples begin to change
+ if (startFound || curSample < firstSample-T55xx_READ_TOL){
+ if (!startFound) dest[i++] = firstSample;
+ startFound = true;
+ dest[i++] = curSample;
+ LED_D_OFF();
+ if (i >= bufferlength) break;
+ }