- /* real time difference in timer unit (resolution 4us) */
- diff = (best_cpt * 16384L) + (icr_diff & 0x3fff);
- current_motor_period = diff; /* save it in global var */
- return 250000000L/diff;
+ current_motor_period = icr_diff;
+ return TIM3_UNIT/icr_diff;
+}
+
+static int8_t check_sta_frame(uint16_t frame, uint16_t time)
+{
+ int8_t beacon_id;
+ uint16_t cksum;
+
+ /* ignore bad cksum */
+ cksum = verify_cksum(frame);
+ if (cksum == 0xFFFF)
+ goto fail;
+
+ beacon_id = frame & 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, cksum=%x\r\n",
+ beacon_id, frame, time, cksum);
+ }
+ return -1;