From: Olivier Matz <zer0@droids-corp.org>
Date: Mon, 8 Mar 2010 22:28:54 +0000 (+0100)
Subject: linear interpolation, prepare multi TSOP
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=7bca487a2198825b6d7caabcd1143e2d5e19dd43;p=aversive.git
linear interpolation, prepare multi TSOP
---
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<<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 */
@@ -146,7 +154,6 @@ void debug_tsop(void)
#endif
}
-#if 0
/* 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)
@@ -169,77 +176,88 @@ static uint16_t verify_cksum(uint16_t val)
return x;
return 0xffff; /* wrong cksum */
}
-#endif
+
+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 (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 <aversive.h>"
+print "#include <aversive/pgmspace.h>"
+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 <aversive.h>
#include <aversive/wait.h>
+#include <aversive/pgmspace.h>
#include <uart.h>
@@ -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);