+ current_motor_period = diff; /* save it in global var */
+#endif
+
+ /* too slow (less than 1 tr/s) */
+ if (icr_cpt >= CPT_ICR_MAX)
+ return 1000L;
+
+ /* too fast (more than 100 tr/s) */
+ if (icr_cpt <= CPT_ICR_MIN)
+ return 100000L;
+
+ /* XXX test */
+ if (icr_cpt > 25)
+ return icr_cpt * 8192UL;
+
+ return TIM3_UNIT/icr_diff;
+}
+
+static int8_t check_sta_frame(uint16_t frame, uint16_t time)
+{
+ int8_t beacon_id;
+
+ /* ignore bad cksum */
+ if (verify_cksum(frame) == 0xFFFF)
+ goto fail;
+
+ beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK;
+
+ if (beacon_id != TSOP_STA_BEACON_ID0 &&
+ beacon_id != TSOP_STA_BEACON_ID1)
+ goto fail;
+
+ /* if motor speed is not good, skip values */
+ if (current_motor_period < MOTOR_PERIOD_MIN)
+ goto fail;
+ if (current_motor_period > MOTOR_PERIOD_MAX)
+ goto fail;
+
+ return beacon_id;
+
+ fail:
+ /* display if needed */
+ if (beacon_tsop.debug_frame) {
+ printf("STA ID=%d frame=%x time=%d\r\n",
+ beacon_id, frame, time);
+ }
+ return -1;
+}
+
+
+/* process the received frame ring */
+static void process_sta_ring(struct frame_status *status)
+{
+ uint8_t head, head_next;
+ uint16_t frame, frametick;
+ uint8_t found = 0;
+ int8_t beacon_id;
+
+ /* beacon 0 */
+ uint16_t data0, time0, ref_time0;
+ double angle0;
+ double dist0;
+
+ /* beacon 1 */
+ uint16_t data1, time1, ref_time1;
+ double angle1;
+ double dist1;
+
+ point_t pos;
+ double a;
+
+ /* remove too old captures from the ring */
+ while (status->head != status->tail) {
+ head_next = (status->head+1) & FRAME_RING_MASK;
+ frametick = status->ring[status->head].tick;
+ if ((uint16_t)(tick - frametick) < MAX_CAP_AGE)
+ break;
+ status->head = head_next;
+ }
+
+ head = status->head;
+ /* after CS, check if we have a new frame in ring */
+ while (head != status->tail) {
+ head_next = (head+1) & FRAME_RING_MASK;
+ frame = status->ring[head].frame;
+
+ beacon_id = check_sta_frame(frame, status->ring[head].time);
+ if (beacon_id < 0) {
+ head = head_next;
+ continue;
+ }
+
+ if (beacon_id == TSOP_STA_BEACON_ID0) {
+ found |= 0x1;
+ data0 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK;
+ time0 = status->ring[head].time;
+ ref_time0 = status->ring[head].ref_time;
+ }
+ else if (beacon_id == TSOP_STA_BEACON_ID1) {
+ found |= 0x2;
+ data1 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK;
+ time1 = status->ring[head].time;
+ ref_time1 = status->ring[head].ref_time;
+ }
+
+ head = head_next;
+ }
+
+ /* if we didn't found beacon 0 and 1, return */
+ if (found != 0x3)
+ return;
+
+ /* update ring head */
+ status->head = head;
+
+ /* beacon 0 */
+ dist0 = data0;
+ dist0 /= 512.;
+ dist0 *= (MAX_DIST-MIN_DIST);
+ dist0 += MIN_DIST;
+
+ time0 = time0 - ref_time0;
+ angle0 = (double)time0 / (double)current_motor_period;
+ if (angle0 > 1.)
+ angle0 -= 1.;
+ if (angle0 > 1.)
+ return; /* fail */
+ angle0 *= (2 * M_PI);
+ if (angle0 > M_PI)
+ angle0 -= M_PI;
+
+ /* beacon 1 */
+ dist1 = data1;
+ dist1 /= 512.;
+ dist1 *= (MAX_DIST-MIN_DIST);
+ dist1 += MIN_DIST;
+
+ time1 = time1 - ref_time1;
+ angle1 = (double)time1 / (double)current_motor_period;
+ if (angle1 > 1.)
+ angle1 -= 1.;
+ if (angle1 > 1.)
+ return; /* fail */
+ angle1 *= (2 * M_PI);
+ if (angle0 > M_PI)
+ angle0 -= M_PI;
+
+ /* display if needed */
+ if (beacon_tsop.debug_frame) {
+ printf("STA ID=%d dist0=%2.2f angle0=%2.2f dist1=%2.2f angle1=%2.2f\r\n",
+ beacon_id, dist0, angle0 * 180. / M_PI, dist1, angle1 * 180. / M_PI);
+ }
+
+ if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, angle0, dist0,
+ angle1, dist1) < 0)
+ return;
+
+ xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)a);
+}
+
+static int8_t check_opp_frame(uint16_t frame, uint16_t time)
+{
+ int8_t beacon_id = -1;
+
+ /* ignore bad cksum */
+ if (verify_cksum(frame) == 0xFFFF)
+ goto fail;
+
+ beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK;
+ if (beacon_id != TSOP_OPP_BEACON_ID)
+ goto fail;
+
+ /* if motor speed is not good, skip values */
+ if (current_motor_period < MOTOR_PERIOD_MIN)
+ goto fail;
+ if (current_motor_period > MOTOR_PERIOD_MAX)
+ goto fail;
+
+ return beacon_id;
+ fail:
+ /* display if needed */
+ if (beacon_tsop.debug_frame) {
+ printf("OPP ID=%d frame=%x time=%d\r\n",
+ beacon_id, frame, time);
+ }
+ return -1;
+}
+
+/* process the received frame ring */
+static void process_opp_ring(struct frame_status *status)
+{
+ uint8_t head_next;
+ uint16_t frame;
+ uint8_t found = 0;
+ uint16_t data, time, ref_time;
+ double angle;
+ double dist;
+
+ /* after CS, check if we have a new frame in ring */
+ while (status->head != status->tail) {
+ head_next = (status->head+1) & FRAME_RING_MASK;
+ frame = status->ring[status->head].frame;
+
+ if (check_opp_frame(frame, status->ring[status->head].time) < 0) {
+ status->head = head_next;
+ continue;
+ }
+
+ found = 1;
+ data = (frame >> TSOP_OPP_FRAME_DATA_SHIFT) & TSOP_OPP_FRAME_DATA_MASK;
+ time = status->ring[status->head].time;
+ ref_time = status->ring[status->head].ref_time;
+
+ status->head = head_next;
+ }
+
+ if (found == 0)
+ return;
+
+ dist = data;
+ dist /= 512.;
+ dist *= (MAX_DIST-MIN_DIST);
+ dist += MIN_DIST;
+
+ time = time - ref_time;
+ angle = (double)time / (double)current_motor_period;
+ if (angle > 1.)
+ angle -= 1.;
+ if (angle > 1.)
+ return; /* fail */
+ angle *= 3600; /* angle in 1/10 deg */
+
+ /* display if needed */
+ if (beacon_tsop.debug_frame) {
+ printf("OPP dist=%2.2f angle=%2.2f\r\n", dist, angle/10);
+ }
+ xmit_opp((uint16_t)dist, (uint16_t)angle);