- frame |= ((uint32_t)BEACON_ID << BEACON_ID_SHIFT);
- frame |= ((uint32_t)(laserdiff & FRAME_DATA_MASK) << FRAME_DATA_SHIFT);
- return frame;
+ uint16_t val, mid, min, max;
+
+ /* for calibration, return the time */
+#ifdef INC_FRAME
+ static uint16_t fr = 1;
+ fr += 2;
+ return fr;
+#endif
+#ifdef SEND_RAWTIME
+ return laserdiff | 1; /* frame must be odd */
+#endif
+
+ min = 0;
+ max = 511;
+ while (min != max) {
+ mid = (max + min + 1) / 2;
+ val = pgm_read_word(&framedist_table[mid]);
+
+ if (laserdiff > val)
+ max = mid - 1;
+ else
+ min = mid;
+ }
+
+ frame |= ((uint32_t)((min << FRAME_DATA_SHIFT)
+ & FRAME_DATA_MASK));
+ frame |= BEACON_ID;
+
+ /* process cksum and return */
+ return do_cksum(frame);