X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Fbeacon_tsop%2Fmain.c;h=0cf5319e95f27029c9deed708e082e728ffec261;hp=ce287ccacca815f7538377d434b9c5e6b94804c1;hb=7bca487a2198825b6d7caabcd1143e2d5e19dd43;hpb=dccae820e0f5c68a3a9c496b63eb8d02102d6ebc

diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c
index ce287cc..0cf5319 100755
--- a/projects/microb2010/tests/beacon_tsop/main.c
+++ b/projects/microb2010/tests/beacon_tsop/main.c
@@ -32,6 +32,12 @@
 #include "cmdline.h"
 #include "main.h"
 
+/* beacon identifier: must be odd, 3 bits */
+#define BEACON_ID_MASK 0x7
+#define BEACON_ID_SHIFT 0
+#define FRAME_DATA_MASK  0xFF8
+#define FRAME_DATA_SHIFT 3
+
 /******************* TSOP */
 
 #define EICRx_TSOP EICRB /* EICRA is not ok, cannot do intr on any edge */
@@ -40,18 +46,27 @@
 #define ISCx0_TSOP ISC60
 #define ISCx1_TSOP ISC61
 #define SIG_TSOP   SIG_INTERRUPT6
-#define TSOP_READ() (PINE & 0x40)
+#define TSOP_READ() (!(PINE & 0x40))
 #else
 #define INTx_TSOP  INT4
 #define ISCx0_TSOP ISC40
 #define ISCx1_TSOP ISC41
 #define SIG_TSOP   SIG_INTERRUPT4
-#define TSOP_READ() (PINE & 0x10)
+#define TSOP_READ() (!(PINE & 0x10))
 #endif
 
+//#define MODUL_455KHZ
+#define MODUL_38KHZ
+
+#if (defined MODUL_455KHZ)
 #define TSOP_FREQ_MHZ 0.455
-#define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ)
 #define N_PERIODS   10.
+#else
+#define TSOP_FREQ_MHZ 0.038
+#define N_PERIODS   15.
+#endif
+
+#define TSOP_PERIOD_US (1./TSOP_FREQ_MHZ)
 
 #define TSOP_TIME_SHORT_US (1.5 * N_PERIODS * TSOP_PERIOD_US)
 #define TSOP_TIME_LONG_US  (2.5 * N_PERIODS * TSOP_PERIOD_US)
@@ -60,25 +75,34 @@
 #define TSOP_TIME_LONG  ((uint16_t)(TSOP_TIME_LONG_US*2))
 
 #define FRAME_LEN 16
-
-/* frame */
-static uint16_t start_angle_time;
-static uint16_t frame;
-static uint16_t mask;
-static uint8_t len;
-static uint8_t val;
+#define FRAME_MASK ((1UL << FRAME_LEN) - 1)
 
 struct detected_frame {
 	uint16_t frame;
 	uint16_t time;
 };
 
+/* frame */
+struct frame_status {
+	uint8_t led_cpt;
+	uint16_t start_angle_time;
+	uint16_t frame;
+	uint16_t mask;
+	uint16_t prev_time;
+	uint8_t prev_tsop;
+	uint8_t len;
+	uint8_t val;
 #define FRAME_RING_ORDER 4
 #define FRAME_RING_SIZE  (1<<FRAME_RING_ORDER)
 #define FRAME_RING_MASK  (FRAME_RING_SIZE-1)
-static uint8_t frame_ring_head = 0;
-static uint8_t frame_ring_tail = 0;
-static struct detected_frame frame_ring[FRAME_RING_SIZE];
+	uint8_t head;
+	uint8_t tail;
+	struct detected_frame ring[FRAME_RING_SIZE];
+};
+
+static struct frame_status static_beacon;
+
+
 
 /********************** CS */
 
@@ -130,66 +154,110 @@ void debug_tsop(void)
 #endif
 }
 
-/* decode frame */
-SIGNAL(SIG_TSOP) {
-	static uint8_t led_cpt = 0;
-
-	/* tsop status */
-	static uint8_t prev_tsop = 0;
-	uint8_t cur_tsop;
-
-	/* time */
-	static uint16_t prev_time;
-	uint16_t ref_time;
-	uint16_t cur_time;
-	uint16_t diff_time;
+/* val is 16 bits, including 4 bits-cksum in MSB, return 0xFFFF is
+ * cksum is wrong, or the 12 bits value on success. */
+static uint16_t verify_cksum(uint16_t val)
+{
+	uint16_t x, cksum;
+
+	x = (val & 0xfff);
+	/* add the four 4-bits blocks of val together */
+	cksum = val & 0xf;
+	val = val >> 4;
+	cksum += val & 0xf;
+	cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
+	val = val >> 4;
+	cksum += val & 0xf;
+	cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
+	val = val >> 4;
+	cksum += val & 0xf;
+	cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
+	if (cksum == 0xf)
+		return x;
+	return 0xffff; /* wrong cksum */
+}
 
-	ref_time = ICR3;
-	cur_time = TCNT3;
-	cur_tsop = TSOP_READ();
-	diff_time = cur_time - prev_time;
+static inline void decode_frame(struct frame_status *status,
+				uint16_t ref_time, uint16_t cur_time, uint8_t cur_tsop)
+{
+	uint16_t diff_time = cur_time - status->prev_time;
 
 	/* first rising edge */
-	if (cur_tsop && diff_time > TSOP_TIME_LONG) {
-		len = 1;
-		val = 1;
-		frame = 0;
-		start_angle_time = cur_time - ref_time;
-		mask = 1;
+	if (status->len == 0 && cur_tsop && diff_time > TSOP_TIME_LONG) {
+		status->len = 1;
+		status->val = 1;
+		status->frame = 0;
+		status->start_angle_time = cur_time - ref_time;
+		status->mask = 1;
 	}
 	/* any short edge */
-	else if (diff_time < TSOP_TIME_SHORT) {
-		if (len & 1) {
-			if (val)
-				frame |= mask;
-			mask <<= 1;
+	else if (status->len != 0 && diff_time < TSOP_TIME_SHORT) {
+		if (status->len & 1) {
+			if (status->val)
+				status->frame |= status->mask;
+			status->mask <<= 1;
 		}
-		len ++;
+		status->len ++;
 	}
 	/* any long edge */
-	else if (diff_time < TSOP_TIME_LONG) {
-		val = !val;
-		if (val)
-			frame |= mask;
-		mask <<= 1;
-		len += 2;
+	else if (status->len != 0 && diff_time < TSOP_TIME_LONG) {
+		status->val = !status->val;
+		if (status->val)
+			status->frame |= status->mask;
+		status->mask <<= 1;
+		status->len += 2;
+	}
+	/* error case, reset */
+	else {
+		status->len = 0;
 	}
 
 	/* end of frame */
-	if (len == FRAME_LEN*2) {
-		uint8_t tail_next = (frame_ring_tail+1) & FRAME_RING_MASK;
-		if (tail_next != frame_ring_head) {
-			frame_ring[frame_ring_tail].frame = frame;
-			frame_ring[frame_ring_tail].time = start_angle_time;
-			frame_ring_tail = tail_next;
+	if (status->len == FRAME_LEN*2) {
+		uint8_t tail_next = (status->tail+1) & FRAME_RING_MASK;
+
+		if (tail_next != status->head) {
+			status->ring[status->tail].frame = (status->frame & FRAME_MASK);
+			status->ring[status->tail].time = status->start_angle_time;
+			status->tail = tail_next;
+			if ((status->led_cpt & 0x7) == 0)
+				LED3_TOGGLE();
+			status->led_cpt ++;
 		}
-		if (led_cpt & 0x8)
-			LED3_TOGGLE();
-		led_cpt ++;
+		status->len = 0;
 	}
 
-	prev_time = cur_time;
-	prev_tsop = cur_tsop;
+	status->prev_time = cur_time;
+	status->prev_tsop = cur_tsop;
+}
+
+/* decode frame */
+SIGNAL(SIG_TSOP) {
+	static uint8_t running = 0;
+
+	/* tsop status */
+	uint8_t cur_tsop;
+	uint16_t ref_time;
+	uint16_t cur_time;
+
+	ref_time = ICR3;
+	cur_time = TCNT3;
+	cur_tsop = TSOP_READ();
+
+	/* avoid interruption stacking */
+	if (running)
+		return;
+	running = 1;
+	sei();
+
+	if (cur_tsop)
+		LED2_ON();
+	else
+		LED2_OFF();
+
+	decode_frame(&static_beacon, ref_time, cur_time, cur_tsop);
+
+	running = 0;
 }
 
 /* absolute value */
@@ -241,6 +309,36 @@ static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff)
 	return 2000000000L/diff;
 }
 
+/* process the received frame ring */
+void process_ring(struct frame_status *status)
+{
+	uint8_t head_next;
+	uint32_t frame;
+
+	/* 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;
+
+		/* display if needed */
+		if (beacon_tsop.debug_frame) {
+			uint8_t beacon_id;
+			uint16_t data;
+				
+			/* ignore bad cksum */
+			if (verify_cksum(frame) == 0xFFFF)
+				continue;
+
+			beacon_id = (frame >> BEACON_ID_SHIFT) & BEACON_ID_MASK;
+			data = (frame >> FRAME_DATA_SHIFT) & FRAME_DATA_MASK;
+			printf("ID=%d data=%d time=%d\r\n",
+			       beacon_id, data,
+			       status->ring[status->head].time);
+		}
+		status->head = head_next;
+	}
+}
+
 int main(void)
 {
 	uint16_t prev_cs = 0;
@@ -314,7 +412,7 @@ int main(void)
 			sei();
 			ETIFR = _BV(ICF3);
 
-			LED2_TOGGLE();
+			//LED2_TOGGLE();
 			diff_icr = (icr - prev_icr);
 			cpt_icr = cpt;
 			prev_icr = icr;
@@ -370,16 +468,7 @@ int main(void)
 		if (cpt < CPT_ICR_MAX)
 			cpt ++;
 
-		/* after CS, check if we have a new frame in ring */
-		if (frame_ring_head != frame_ring_tail) {
-			uint8_t head_next;
-			head_next = (frame_ring_head+1) & FRAME_RING_MASK;
-			if (beacon_tsop.debug_frame)
-				printf("%x %d\n", frame_ring[frame_ring_head].frame,
-				       frame_ring[frame_ring_head].time);
-			frame_ring_head = head_next;
-		}
-
+		process_ring(&static_beacon);
 	}
 
 	return 0;