From 7bca487a2198825b6d7caabcd1143e2d5e19dd43 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Mon, 8 Mar 2010 23:28:54 +0100 Subject: [PATCH] linear interpolation, prepare multi TSOP --- projects/microb2010/tests/beacon_tsop/main.c | 198 ++++++++++-------- .../microb2010/tests/static_beacon/Makefile | 2 +- .../microb2010/tests/static_beacon/coding.py | 94 +++++++-- .../tests/static_beacon/static_beacon.c | 39 ++-- 4 files changed, 207 insertions(+), 126 deletions(-) diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c index edc444b..0cf5319 100755 --- a/projects/microb2010/tests/beacon_tsop/main.c +++ b/projects/microb2010/tests/beacon_tsop/main.c @@ -35,7 +35,7 @@ /* beacon identifier: must be odd, 3 bits */ #define BEACON_ID_MASK 0x7 #define BEACON_ID_SHIFT 0 -#define FRAME_DATA_MASK 0xFFF8 +#define FRAME_DATA_MASK 0xFF8 #define FRAME_DATA_SHIFT 3 /******************* TSOP */ @@ -77,24 +77,32 @@ #define FRAME_LEN 16 #define FRAME_MASK ((1UL << FRAME_LEN) - 1) -/* frame */ -static uint16_t start_angle_time; -static uint16_t frame; -static uint16_t mask; -static uint8_t len; -static uint8_t val; - 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<prev_time; + + /* first rising edge */ + 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 (status->len != 0 && diff_time < TSOP_TIME_SHORT) { + if (status->len & 1) { + if (status->val) + status->frame |= status->mask; + status->mask <<= 1; + } + status->len ++; + } + /* any long edge */ + 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 (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 ++; + } + status->len = 0; + } + + status->prev_time = cur_time; + status->prev_tsop = cur_tsop; +} /* decode frame */ SIGNAL(SIG_TSOP) { - static uint8_t led_cpt = 0; + static uint8_t running = 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; ref_time = ICR3; cur_time = TCNT3; cur_tsop = TSOP_READ(); - diff_time = cur_time - prev_time; + + /* avoid interruption stacking */ + if (running) + return; + running = 1; + sei(); if (cur_tsop) LED2_ON(); else LED2_OFF(); - /* first rising edge */ - if (len == 0 && cur_tsop && diff_time > TSOP_TIME_LONG) { - len = 1; - val = 1; - frame = 0; - start_angle_time = cur_time - ref_time; - mask = 1; - } - /* any short edge */ - else if (len != 0 && diff_time < TSOP_TIME_SHORT) { - if (len & 1) { - if (val) - frame |= mask; - mask <<= 1; - } - len ++; - } - /* any long edge */ - else if (len != 0 && diff_time < TSOP_TIME_LONG) { - val = !val; - if (val) - frame |= mask; - mask <<= 1; - len += 2; - } - /* error case, reset */ - else { - len = 0; - } + decode_frame(&static_beacon, ref_time, cur_time, cur_tsop); - /* 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_MASK); - frame_ring[frame_ring_tail].time = start_angle_time; - frame_ring_tail = tail_next; - } - if ((led_cpt & 0x7) == 0) - LED3_TOGGLE(); - led_cpt ++; - } - - prev_time = cur_time; - prev_tsop = cur_tsop; + running = 0; } /* absolute value */ @@ -291,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; @@ -420,27 +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; - uint32_t frame; - head_next = (frame_ring_head+1) & FRAME_RING_MASK; - frame = frame_ring[frame_ring_head].frame; - - /* display if needed */ - if (beacon_tsop.debug_frame) { - uint8_t beacon_id; - uint16_t data; - - 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, - frame_ring[frame_ring_head].time); - } - frame_ring_head = head_next; - } - + process_ring(&static_beacon); } return 0; diff --git a/projects/microb2010/tests/static_beacon/Makefile b/projects/microb2010/tests/static_beacon/Makefile index ddbfbda..93641fa 100755 --- a/projects/microb2010/tests/static_beacon/Makefile +++ b/projects/microb2010/tests/static_beacon/Makefile @@ -3,7 +3,7 @@ TARGET = static_beacon AVERSIVE_DIR = ../../../.. # List C source files here. (C dependencies are automatically generated.) -SRC = $(TARGET).c +SRC = framedist.c $(TARGET).c # List Assembler source files here. # Make them always end in a capital .S. Files ending in a lowercase .s diff --git a/projects/microb2010/tests/static_beacon/coding.py b/projects/microb2010/tests/static_beacon/coding.py index 440fd3b..a41c692 100644 --- a/projects/microb2010/tests/static_beacon/coding.py +++ b/projects/microb2010/tests/static_beacon/coding.py @@ -2,7 +2,13 @@ import sys, math -RPS = 10. +if 1: + RPS = 10. + TIMER_FREQ = 2000000. +else: + RPS = 40. + TIMER_FREQ = 16000000. + LASER_RADIUS = 25. # mm MIN = 200. @@ -11,6 +17,19 @@ NBITS = 9 STEPS = (1 << 9) k = math.pow(MAX/MIN, 1./STEPS) +def mm_to_framedist(mm): + d = mm + d -= MIN + d /= (MAX-MIN) + d *= 512 + return d + +def framedist_to_mm(d): + d /= 512. + d *= (MAX-MIN) + d += MIN + return d + # t is in us, result is 9 bits def time_to_frame(t): # process angle from t @@ -18,20 +37,65 @@ def time_to_frame(t): # process d from a (between 20cm and 350cm) d = LASER_RADIUS / math.sin(a/2) - - frame = math.log(d/MIN)/math.log(k) - if frame >= 512: - frame = 511 - else: - frame = int(frame) - print frame + frame = int(mm_to_framedist(d)) return frame -# frame is integer 9 bits, result is distance -def frame_to_distance(frame): - d = MIN*(math.pow(k, frame)) - print d - return d +# frame is integer 9 bits, result is laserdiff time +def frame_to_time(frame): + d = framedist_to_mm(frame) + a = 2 * math.asin(LASER_RADIUS/d) + t = (a * (TIMER_FREQ/RPS)) / (2. * math.pi) + return t + +def sample_to_offset(samples, table): + offsets = samples[:] + for i in range(len(offsets)): + o = offsets[i] + framedist = mm_to_framedist(o[0]) + off = o[1] - table[int(framedist)] + offsets[i] = framedist, off + return offsets + +def linear_interpolation(offsets, framedist, time): + if framedist <= offsets[0][0]: + return time + offsets[0][1] + if framedist >= offsets[-1][0]: + return time + offsets[-1][1] + + #print (offsets, framedist, time) + o_prev = offsets[0] + for o in offsets[1:]: + if framedist > o[0]: + o_prev = o + continue + x = (framedist - o_prev[0]) / (o[0] - o_prev[0]) + return time + o_prev[1] + (x * (o[1] - o_prev[1])) + return None + +#x = time_to_frame(float(sys.argv[1])) +#frame_to_distance(x) +#frame_to_time(int(sys.argv[1])) + + +table = [0] * 512 +for i in range(512): + table[i] = frame_to_time(i) + +# linear correction: distance_mm, time +samples = [ + (250., 7600.), + (500., 3000.), + (3000., 400.), + ] -x = time_to_frame(float(sys.argv[1])) -frame_to_distance(x) +offsets = sample_to_offset(samples, table) +print "#include " +print "#include " +print "prog_uint16_t framedist_table[] = {" +for i in range(512): + if (i % 8) == 0: + print " ", + print "%d,"%(int(linear_interpolation(offsets, i, table[i]))), + if (i % 8 == 7): + print +print "};" diff --git a/projects/microb2010/tests/static_beacon/static_beacon.c b/projects/microb2010/tests/static_beacon/static_beacon.c index 9f9c8d6..d2accb3 100755 --- a/projects/microb2010/tests/static_beacon/static_beacon.c +++ b/projects/microb2010/tests/static_beacon/static_beacon.c @@ -24,6 +24,7 @@ #include #include +#include #include @@ -130,6 +131,7 @@ #error "speed not defined" #endif +extern prog_uint16_t framedist_table[]; /* basic functions to transmit on IR */ static inline void xmit_0(void) @@ -216,33 +218,20 @@ static uint16_t do_cksum(uint16_t val) static uint32_t get_frame(uint16_t laserdiff) { uint32_t frame = 0; - double a, d; - uint16_t frame_dist; + uint16_t frame_dist = 256; + uint16_t step, val; - frame |= ((uint32_t)BEACON_ID << BEACON_ID_SHIFT); + /* for calibration, return the time */ + if (0) + return laserdiff; - /* process angle from laserdiff time */ -#ifdef SPEED_10RPS - /* timer = 2Mhz */ - a = ((double)laserdiff / (2000000./10.)) * 2. * M_PI; -#else - /* timer = 16Mhz */ - a = ((double)laserdiff / (16000000./40.)) * 2. * M_PI; -#endif - /* get distance from angle */ - d = LASER_DIST / sin(a/2); - - /* scale it between 0 and 511 */ - if (d <= MIN_DIST) - return 0; - if (d >= MAX_DIST) - return 0; - d -= MIN_DIST; - d /= (MAX_DIST-MIN_DIST); - d *= 512; - frame_dist = (uint16_t)d; - if (frame_dist >= 512) /* should not happen... */ - return 0; + for (step = 128; step != 0; step /= 2) { + val = pgm_read_word(&framedist_table[frame_dist]); + if (laserdiff > val) + frame_dist -= step; + else + frame_dist += step; + } frame |= ((uint32_t)(frame_dist & FRAME_DATA_MASK) << FRAME_DATA_SHIFT); -- 2.39.5