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 (executable)
--- 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
 /* 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 */
 #define FRAME_DATA_SHIFT 3
 
 /******************* TSOP */


@@ -77,24 +77,32 @@
 #define FRAME_LEN 16
 #define FRAME_MASK ((1UL << FRAME_LEN) - 1)
 
 #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;
 };
 
 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)
 #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 */
 
 
 /********************** CS */
 


@@ -146,7 +154,6 @@ void debug_tsop(void)
 #endif
 }
 
 #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)
 /* 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 */
 }
                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) {
 
 /* decode frame */
 SIGNAL(SIG_TSOP) {

-       static uint8_t led_cpt = 0;
+       static uint8_t running = 0;

 
        /* tsop status */
 
        /* tsop status */

-       static uint8_t prev_tsop = 0;

        uint8_t cur_tsop;
        uint8_t cur_tsop;

-
-       /* time */
-       static uint16_t prev_time;

        uint16_t ref_time;
        uint16_t cur_time;
        uint16_t ref_time;
        uint16_t cur_time;

-       uint16_t diff_time;

 
        ref_time = ICR3;
        cur_time = TCNT3;
        cur_tsop = TSOP_READ();
 
        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();
 
 
        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 */
 }
 
 /* absolute value */


@@ -291,6 +309,36 @@ static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff)
        return 2000000000L/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;
 int main(void)
 {
        uint16_t prev_cs = 0;


@@ -420,27 +468,7 @@ int main(void)
                if (cpt < CPT_ICR_MAX)
                        cpt ++;
 
                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;
        }
 
        return 0;

diff --git a/projects/microb2010/tests/static_beacon/Makefile b/projects/microb2010/tests/static_beacon/Makefile
index ddbfbda..93641fa 100755 (executable)
--- 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.)
 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
 
 # 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 (file)
--- a/projects/microb2010/tests/static_beacon/coding.py
+++ b/projects/microb2010/tests/static_beacon/coding.py
@@ -2,7 +2,13 @@
 
 import sys, math
 
 
 import sys, math
 

-RPS = 10.
+if 1:
+    RPS = 10.
+    TIMER_FREQ = 2000000.
+else:
+    RPS = 40.
+    TIMER_FREQ = 16000000.
+

 LASER_RADIUS = 25. # mm
 
 MIN = 200.
 LASER_RADIUS = 25. # mm
 
 MIN = 200.


@@ -11,6 +17,19 @@ NBITS = 9
 STEPS = (1 << 9)
 k = math.pow(MAX/MIN, 1./STEPS)
 
 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
 # 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)
 
     # 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
 
     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 (executable)
--- 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.h>
 #include <aversive/wait.h>

+#include <aversive/pgmspace.h>

 
 #include <uart.h>
 
 
 #include <uart.h>
 


@@ -130,6 +131,7 @@
 #error "speed not defined"
 #endif
 
 #error "speed not defined"
 #endif
 

+extern prog_uint16_t framedist_table[];

 /* basic functions to transmit on IR */
 
 static inline void xmit_0(void)
 /* 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;
 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);
 
 
        frame |= ((uint32_t)(frame_dist & FRAME_DATA_MASK) << FRAME_DATA_SHIFT);