static and tourel beacon

diff --git a/projects/microb2010/tests/beacon_tsop/board2006.h b/projects/microb2010/tests/beacon_tsop/board2006.h
index 1b3f67caa6b8a36370bf98f2c5450931fdb192ef..841ab1542e55ac922b5f87ca88d5282aa119d12d 100644 (file)
--- a/projects/microb2010/tests/beacon_tsop/board2006.h
+++ b/projects/microb2010/tests/beacon_tsop/board2006.h
@@ -69,18 +69,18 @@
 #define SIG_TSOP_STA   SIG_INTERRUPT7
 #define TSOP_STA_READ() (!(PINE & 0x80))
 
-#define TSOP_FREQ_455_MHZ 0.455
+#define TSOP_FREQ_455_KHZ 0.455
 #define N_PERIODS_455   10.
-#define TSOP_FREQ_38_MHZ 0.038
+#define TSOP_FREQ_38_KHZ 0.038
 #define N_PERIODS_38   15.
-#define TSOP_FREQ_30_MHZ 0.030
+#define TSOP_FREQ_30_KHZ 0.030
 #define N_PERIODS_30   15.
-#define TSOP_FREQ_56_MHZ 0.056
+#define TSOP_FREQ_56_KHZ 0.056
 #define N_PERIODS_56   15.
 
 /* TSOP STATIC */
 
-#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_38_MHZ)
+#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_38_KHZ)
 #define TSOP_STA_N_PERIODS  (N_PERIODS_38)
 
 #define TSOP_STA_TIME_SHORT_US (1.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US)
@@ -102,7 +102,7 @@
 
 /* TSOP OPP */
 
-#define TSOP_OPP_PERIOD_US (1./TSOP_FREQ_455_MHZ)
+#define TSOP_OPP_PERIOD_US (1./TSOP_FREQ_455_KHZ)
 #define TSOP_OPP_N_PERIODS  (N_PERIODS_455)
 
 #define TSOP_OPP_TIME_SHORT_US (1.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US)

diff --git a/projects/microb2010/tests/beacon_tsop/board2010.h b/projects/microb2010/tests/beacon_tsop/board2010.h
index f66faa173be878ed2a9a5d6aa37922861c6ee0d2..e55856117bd2d955528acd6248afa1fb7f7a4857 100644 (file)
--- a/projects/microb2010/tests/beacon_tsop/board2010.h
+++ b/projects/microb2010/tests/beacon_tsop/board2010.h
@@ -76,18 +76,18 @@
 #define SIG_TSOP_STA   SIG_INTERRUPT5
 #define TSOP_STA_READ() (!(PINE & 0x20))
 
-#define TSOP_FREQ_455_MHZ 0.455
+#define TSOP_FREQ_455_KHZ 0.455
 #define N_PERIODS_455   15.
-#define TSOP_FREQ_38_MHZ 0.038
+#define TSOP_FREQ_38_KHZ 0.038
 #define N_PERIODS_38   15.
-#define TSOP_FREQ_30_MHZ 0.030
+#define TSOP_FREQ_30_KHZ 0.030
 #define N_PERIODS_30   15.
-#define TSOP_FREQ_56_MHZ 0.056
+#define TSOP_FREQ_56_KHZ 0.056
 #define N_PERIODS_56   15.
 
 /* TSOP STATIC */
 
-#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_455_MHZ)
+#define TSOP_STA_PERIOD_US (1./TSOP_FREQ_455_KHZ)
 #define TSOP_STA_N_PERIODS  (N_PERIODS_455)
 
 #define TSOP_STA_TIME_SHORT_US (1.5 * TSOP_STA_N_PERIODS * TSOP_STA_PERIOD_US)
@@ -109,8 +109,8 @@
 
 /* TSOP OPP */
 
-#define TSOP_OPP_PERIOD_US (1./TSOP_FREQ_455_MHZ)
-#define TSOP_OPP_N_PERIODS  (N_PERIODS_455)
+#define TSOP_OPP_PERIOD_US (1./TSOP_FREQ_56_KHZ)
+#define TSOP_OPP_N_PERIODS  (N_PERIODS_56)
 
 #define TSOP_OPP_TIME_SHORT_US (1.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US)
 #define TSOP_OPP_TIME_LONG_US  (2.5 * TSOP_OPP_N_PERIODS * TSOP_OPP_PERIOD_US)

diff --git a/projects/microb2010/tests/beacon_tsop/main.c b/projects/microb2010/tests/beacon_tsop/main.c
index 7d819bbc86653c0e36920fb8fab0778952899f70..2edd5721ada0307edfe628505f4e33e6cda5dbb4 100755 (executable)
--- a/projects/microb2010/tests/beacon_tsop/main.c
+++ b/projects/microb2010/tests/beacon_tsop/main.c
@@ -1,7 +1,7 @@
-/*  
+/*
  *  Copyright Droids Corporation (2010)
  *  Olivier Matz <zer0@droids-corp.org>
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -98,7 +98,7 @@ static uint16_t tick = 0;
 #define CPT_ICR_MIN (uint8_t)((10000UL/(uint32_t)CS_PERIOD_US))   /* too fast = 100 tr/s */
 
 /* in tr / 1000s */
-#define CS_CONSIGN (15 * 1000L)
+#define CS_CONSIGN (20 * 1000L)
 
 /* 5% tolerance to validate captures, period is in  */
 #define TIM3_UNIT 250000000L
@@ -123,7 +123,7 @@ void debug_serial(void)
        while (1) {
                int16_t c;
                c = uart_recv_nowait(0);
-               if (c != -1) 
+               if (c != -1)
                        printf("%c", (char)(c+1));
                LED1_ON();
                wait_ms(500);
@@ -132,7 +132,7 @@ void debug_serial(void)
        }
 #endif
 }
-                 
+
 void debug_tsop(void)
 {
 #if 0
@@ -177,7 +177,6 @@ static inline void decode_frame(struct frame_status *status,
 
        /* first rising edge */
        if (status->len == 0 && cur_tsop && diff_time > status->time_long) {
-               LED6_ON();
                status->len = 1;
                status->val = 1;
                status->frame = 0;
@@ -215,14 +214,11 @@ static inline void decode_frame(struct frame_status *status,
                frame_mask = (1 << status->frame_len) - 1;
 
                if (tail_next != status->head) {
-                       LED5_ON();
                        status->ring[status->tail].frame = (status->frame & frame_mask);
                        status->ring[status->tail].ref_time = status->ref_time;
                        status->ring[status->tail].time = status->start_time;
                        status->ring[status->tail].tick = tick;
                        status->tail = tail_next;
-/*                     if ((status->led_cpt & 0x7) == 0) */
-/*                             LED3_TOGGLE(); */
                        status->led_cpt ++;
                }
                status->len = 0;
@@ -230,10 +226,6 @@ static inline void decode_frame(struct frame_status *status,
 
        status->prev_time = cur_time;
        status->prev_tsop = cur_tsop;
-       LED3_OFF();
-       LED4_OFF();
-       LED5_OFF();
-       LED6_OFF();
 }
 
 /* decode frame */
@@ -255,10 +247,10 @@ SIGNAL(SIG_TSOP_STA) {
        running = 1;
        sei();
 
-/*     if (cur_tsop) */
-/*             LED5_ON(); */
-/*     else */
-/*             LED5_OFF(); */
+       if (cur_tsop)
+               LED5_ON();
+       else
+               LED5_OFF();
 
        decode_frame(&static_beacon, ref_time, cur_time, cur_tsop);
 
@@ -284,12 +276,12 @@ SIGNAL(SIG_TSOP_OPP) {
        running = 1;
        sei();
 
-/*     if (cur_tsop) */
-/*             LED6_ON(); */
-/*     else */
-/*             LED6_OFF(); */
+       if (cur_tsop)
+               LED6_ON();
+       else
+               LED6_OFF();
 
-       //decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop);
+       decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop);
 
        running = 0;
 }
@@ -309,65 +301,33 @@ static inline int32_t AbS(int32_t x)
  *   (modulo 65536 obviously) */
 static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff)
 {
-#if 0
-       int32_t best_diff = 65536L;
-       int8_t best_cpt = -2;
-       int32_t diff;
-       int8_t i;
-
        /* too slow (less than 1 tr/s) */
-       if (icr_cpt > CPT_ICR_MAX)
+       if (icr_cpt >= CPT_ICR_MAX) {
+               current_motor_period = 250000;
                return 1000L;
-
-       /* too fast (more than 100 tr/s) */
-       if (icr_cpt < CPT_ICR_MIN)
-               return 100000L;
-
-       /* try to get the real time knowning icr_cpt and icr_diff */
-       for (i=-1; i<2; i++) {
-               diff = ((icr_cpt+i)&3) * 16384L;
-               diff += (icr_diff & 0x3fff);
-               diff -= icr_diff;
-               if (diff > 32768L)
-                       diff -= 65536L;
-               if (diff < -32768)
-                       diff += 65536L;
-        
-               if (AbS(diff) < AbS(best_diff)) {
-                       best_diff = diff;
-                       best_cpt = icr_cpt + i;
-               }
        }
 
-       /* real time difference in timer unit (resolution 4us) */
-       diff = (best_cpt * 16384L) + (icr_diff & 0x3fff);
-       current_motor_period = diff; /* save it in global var */
-#endif
-
-       /* too slow (less than 1 tr/s) */
-       if (icr_cpt >= CPT_ICR_MAX)
-               return 1000L;
-
        /* too fast (more than 100 tr/s) */
-       if (icr_cpt <= CPT_ICR_MIN)
+       if (icr_cpt <= CPT_ICR_MIN) {
+               current_motor_period = 2500;
                return 100000L;
+       }
 
-       /* XXX test */
-       if (icr_cpt > 25)
-               return icr_cpt * 8192UL;
-
+       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 */
-       if (verify_cksum(frame) == 0xFFFF)
+       cksum = verify_cksum(frame);
+       if (cksum == 0xFFFF)
                goto fail;
 
-       beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK;
+       beacon_id = frame & TSOP_STA_BEACON_ID_MASK;
 
        if (beacon_id != TSOP_STA_BEACON_ID0 &&
            beacon_id != TSOP_STA_BEACON_ID1)
@@ -384,8 +344,8 @@ static int8_t check_sta_frame(uint16_t frame, uint16_t time)
  fail:
        /* display if needed */
        if (beacon_tsop.debug_frame) {
-               printf("STA ID=%d frame=%x time=%d\r\n",
-                      beacon_id, frame, time);
+               printf("STA ID=%d frame=%x time=%d, cksum=%x\r\n",
+                      beacon_id, frame, time, cksum);
        }
        return -1;
 }
@@ -435,13 +395,13 @@ static void process_sta_ring(struct frame_status *status)
 
                if (beacon_id == TSOP_STA_BEACON_ID0) {
                        found |= 0x1;
-                       data0 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK;
+                       data0 = (frame & TSOP_STA_FRAME_DATA_MASK) >> TSOP_STA_FRAME_DATA_SHIFT;
                        time0 = status->ring[head].time;
                        ref_time0 = status->ring[head].ref_time;
                }
                else if (beacon_id == TSOP_STA_BEACON_ID1) {
                        found |= 0x2;
-                       data1 = (frame >> TSOP_STA_FRAME_DATA_SHIFT) & TSOP_STA_FRAME_DATA_MASK;
+                       data1 = (frame & TSOP_STA_FRAME_DATA_MASK) >> TSOP_STA_FRAME_DATA_SHIFT;
                        time1 = status->ring[head].time;
                        ref_time1 = status->ring[head].ref_time;
                }
@@ -504,12 +464,14 @@ static void process_sta_ring(struct frame_status *status)
 static int8_t check_opp_frame(uint16_t frame, uint16_t time)
 {
        int8_t beacon_id = -1;
+       uint16_t cksum;
 
        /* ignore bad cksum */
-       if (verify_cksum(frame) == 0xFFFF)
+       cksum = verify_cksum(frame);
+       if (cksum == 0xFFFF)
                goto fail;
 
-       beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK;
+       beacon_id = frame & TSOP_OPP_BEACON_ID_MASK;
        if (beacon_id != TSOP_OPP_BEACON_ID)
                goto fail;
 
@@ -523,8 +485,10 @@ static int8_t check_opp_frame(uint16_t frame, uint16_t time)
  fail:
        /* display if needed */
        if (beacon_tsop.debug_frame) {
-               printf("OPP ID=%d frame=%x time=%d\r\n",
-                      beacon_id, frame, time);
+               printf("OPP ID=%d frame=%x time=%d cksum=%x d=%d\r\n",
+                      beacon_id, frame, time, cksum,
+                      (frame & TSOP_OPP_FRAME_DATA_MASK) >>
+                      TSOP_OPP_FRAME_DATA_SHIFT);
        }
        return -1;
 }
@@ -538,7 +502,7 @@ static void process_opp_ring(struct frame_status *status)
        uint16_t data, time, ref_time;
        double angle;
        double dist;
-                               
+
        /* after CS, check if we have a new frame in ring */
        while (status->head != status->tail) {
                head_next = (status->head+1) & FRAME_RING_MASK;
@@ -550,7 +514,7 @@ static void process_opp_ring(struct frame_status *status)
                }
 
                found = 1;
-               data = (frame >> TSOP_OPP_FRAME_DATA_SHIFT) & TSOP_OPP_FRAME_DATA_MASK;
+               data = (frame & TSOP_OPP_FRAME_DATA_MASK) >> TSOP_OPP_FRAME_DATA_SHIFT;
                time = status->ring[status->head].time;
                ref_time = status->ring[status->head].ref_time;
 
@@ -619,7 +583,7 @@ int main(void)
 #endif
 
        rdline_init(&beacon_tsop.rdl, write_char, valid_buffer, complete_buffer);
-       snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > ");  
+       snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > ");
        rdline_newline(&beacon_tsop.rdl, beacon_tsop.prompt);
 
        debug_tsop();
@@ -630,7 +594,7 @@ int main(void)
        EIMSK |= _BV(INTx_TSOP_STA) | _BV(INTx_TSOP_OPP);
 
        /* pwm for motor */
-       PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10, 
+       PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
                                 TIMER1_PRESCALER_DIV_1);
 #ifdef BOARD2010
        PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, C, 10, 0, NULL, 0);
@@ -696,7 +660,7 @@ int main(void)
 
                if (cpt == CPT_ICR_MAX)
                        speed = 0;
-               
+
                /* enabled laser when rotation speed if at least 5tr/s */
                if (1 || speed > 5000) /* XXX */
                        LASER_ON();

diff --git a/projects/microb2010/tests/static_beacon/coding.py b/projects/microb2010/tests/static_beacon/coding.py
index 9ef4c5caac918e18fc3050e60db9b29d07d67a4d..05b20abdd1c39c12e888066c5f79f6de86feba2f 100644 (file)
--- a/projects/microb2010/tests/static_beacon/coding.py
+++ b/projects/microb2010/tests/static_beacon/coding.py
@@ -10,7 +10,7 @@ RPS = 20.
 #TIMER_FREQ = 2000000.
 TIMER_FREQ = 16000000.
 
-LASER_RADIUS = 25. # mm
+LASER_RADIUS = 20. # mm
 
 MIN = 200.
 MAX = 3500.
@@ -68,10 +68,12 @@ def time_tick_to_us(t):
 # linear correction: distance_mm, time_us
 # must be ordered
 samples = [
-    (250., 2201.),
-    (450., 701.),
-    (1200., 231.),
-    (3000., 50.),
+    (330.,  15681./16),
+    (778.,  6437./16),
+    (1180., 4351./16),
+    (1608., 3221./16),
+    (2045., 2583./16),
+    (2487., 2167./16),
     ]
 
 dist_mm = map(frame_to_mm, range(512))
@@ -83,7 +85,7 @@ for i in range(512):
 
 # find offset and update theorical curve
 off = samples[-1][1] - mm_to_us(3000.)
-print "offset=%f"%(off)
+#print "offset=%f"%(off)
 theo_off = [0] * 512
 for i in range(512):
     mm = frame_to_mm(i)
@@ -107,33 +109,26 @@ for i in range(512):
     ratio = (mm - samples[smp][0]) / (samples[smp+1][0] - samples[smp][0])
     mm_new = mm_start + ratio * (mm_end - mm_start)
 
+    if mm_new < 0:
+        mm_new = 1.
     final[i] = mm_to_us(mm_new) + off
 
-sample_idx = 0
-while sample_idx < len(samples):
-    print samples[sample_idx][1],
-    print us_to_mm(samples[sample_idx][1] - off),
-    print mm_to_us(samples[sample_idx][0])
-    sample_idx += 1
-
 
 plt.plot(
-#    dist_mm, theorical, "r-",
-#    dist_mm, theo_off, "b-",
+    dist_mm, theorical, "r-",
+    dist_mm, theo_off, "b-",
     dist_mm, final, "g-",
     map(lambda x:x[0], samples), map(lambda x:x[1], samples), "g^",
     )
 plt.show()
 
-
 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]))),
-    print "%d,"%(int(table[i])),
+    print "%d,"%(int(time_us_to_tick(final[i]))),
     if (i % 8 == 7):
         print
 print "};"

diff --git a/projects/microb2010/tests/static_beacon/framedist.c b/projects/microb2010/tests/static_beacon/framedist.c
index eafe45ddc1bfe4dd2e318ed4996d27e29e500382..ed7892a3198d2c6890599caba88f92c4a25268b9 100644 (file)
--- a/projects/microb2010/tests/static_beacon/framedist.c
+++ b/projects/microb2010/tests/static_beacon/framedist.c
@@ -1,68 +1,68 @@
 #include <aversive.h>
 #include <aversive/pgmspace.h>
 prog_uint16_t framedist_table[] = {
-       9074, 8823, 8588, 8367, 8159, 7962, 7776, 7600,
-       7424, 7232, 7047, 6870, 6699, 6535, 6376, 6223,
-       6075, 5932, 5793, 5658, 5528, 5401, 5277, 5157,
-       5041, 4927, 4816, 4707, 4601, 4498, 4397, 4298,
-       4201, 4106, 4013, 3922, 3833, 3745, 3659, 3574,
-       3490, 3409, 3328, 3249, 3171, 3094, 3018, 2958,
-       2919, 2880, 2842, 2805, 2769, 2733, 2699, 2665,
-       2633, 2601, 2569, 2539, 2509, 2479, 2451, 2423,
-       2395, 2368, 2342, 2316, 2291, 2266, 2242, 2218,
-       2195, 2172, 2149, 2127, 2106, 2085, 2064, 2044,
-       2024, 2004, 1985, 1966, 1947, 1929, 1911, 1893,
-       1876, 1858, 1842, 1825, 1809, 1793, 1777, 1762,
-       1746, 1731, 1717, 1702, 1688, 1674, 1660, 1646,
-       1633, 1619, 1606, 1594, 1581, 1568, 1556, 1544,
-       1532, 1520, 1509, 1497, 1486, 1475, 1464, 1453,
-       1442, 1431, 1421, 1411, 1401, 1391, 1381, 1371,
-       1361, 1352, 1342, 1333, 1324, 1315, 1306, 1297,
-       1289, 1280, 1271, 1263, 1255, 1247, 1238, 1230,
-       1223, 1215, 1207, 1199, 1192, 1184, 1177, 1170,
-       1162, 1155, 1148, 1141, 1134, 1127, 1121, 1114,
-       1107, 1101, 1094, 1088, 1082, 1075, 1069, 1063,
-       1057, 1051, 1045, 1039, 1033, 1028, 1022, 1016,
-       1011, 1005, 1000, 994, 989, 984, 978, 973,
-       968, 963, 958, 953, 948, 943, 938, 933,
-       928, 923, 919, 914, 909, 905, 900, 896,
-       891, 887, 883, 878, 874, 870, 866, 861,
-       857, 853, 849, 845, 841, 837, 833, 829,
-       825, 822, 818, 814, 810, 806, 803, 799,
-       796, 792, 788, 785, 781, 778, 774, 771,
-       768, 764, 761, 758, 754, 751, 748, 745,
-       742, 738, 735, 732, 729, 726, 723, 720,
-       717, 714, 711, 708, 705, 702, 700, 697,
-       694, 691, 688, 686, 683, 680, 677, 675,
-       672, 669, 667, 664, 662, 659, 657, 654,
-       652, 649, 647, 644, 642, 639, 637, 635,
-       632, 630, 627, 625, 623, 621, 618, 616,
-       614, 612, 609, 607, 605, 603, 601, 599,
-       596, 594, 592, 590, 588, 586, 584, 582,
-       580, 578, 576, 574, 572, 570, 568, 566,
-       564, 562, 561, 559, 557, 555, 553, 551,
-       550, 548, 546, 544, 542, 541, 539, 537,
-       535, 534, 532, 530, 529, 527, 525, 524,
-       522, 520, 519, 517, 516, 514, 512, 511,
-       509, 508, 506, 505, 503, 502, 500, 499,
-       497, 496, 494, 493, 491, 490, 488, 487,
-       485, 484, 483, 481, 480, 478, 477, 476,
-       474, 473, 472, 470, 469, 468, 466, 465,
-       464, 462, 461, 460, 459, 457, 456, 455,
-       454, 452, 451, 450, 449, 447, 446, 445,
-       444, 443, 442, 440, 439, 438, 437, 436,
-       435, 434, 432, 431, 430, 429, 428, 427,
-       426, 425, 424, 423, 421, 420, 419, 418,
-       417, 416, 415, 414, 413, 412, 411, 410,
-       409, 408, 407, 406, 405, 404, 403, 402,
-       401, 400, 399, 398, 397, 396, 395, 394,
-       393, 392, 391, 389, 388, 387, 386, 385,
-       384, 383, 382, 381, 380, 379, 378, 377,
-       376, 374, 373, 372, 371, 370, 369, 368,
-       367, 366, 365, 364, 363, 362, 361, 360,
-       359, 358, 357, 356, 356, 355, 354, 353,
-       352, 351, 350, 349, 348, 347, 346, 345,
-       344, 343, 342, 342, 341, 340, 339, 338,
-       337, 336, 335, 334, 334, 333, 332, 331,
-       330, 329, 328, 327, 327, 326, 325, 324,
+       29237, 28064, 26983, 25983, 25057, 24195, 23392, 22641,
+       21938, 21279, 20658, 20074, 19523, 19001, 18508, 18040,
+       17596, 17174, 16772, 16389, 16023, 15674, 15341, 15021,
+       14715, 14422, 14141, 13870, 13610, 13360, 13120, 12888,
+       12665, 12449, 12241, 12040, 11846, 11658, 11476, 11300,
+       11130, 10965, 10805, 10650, 10499, 10353, 10211, 10073,
+       9939, 9808, 9681, 9558, 9437, 9320, 9206, 9095,
+       8987, 8881, 8778, 8677, 8579, 8483, 8390, 8298,
+       8209, 8121, 8036, 7952, 7871, 7791, 7713, 7636,
+       7561, 7488, 7416, 7345, 7276, 7209, 7142, 7077,
+       7013, 6951, 6889, 6829, 6770, 6712, 6655, 6599,
+       6544, 6490, 6438, 6387, 6337, 6288, 6239, 6192,
+       6145, 6099, 6053, 6009, 5965, 5922, 5879, 5837,
+       5796, 5756, 5716, 5676, 5638, 5599, 5562, 5525,
+       5488, 5452, 5416, 5381, 5347, 5313, 5279, 5246,
+       5214, 5181, 5150, 5118, 5087, 5057, 5027, 4997,
+       4968, 4939, 4910, 4882, 4854, 4826, 4799, 4772,
+       4746, 4720, 4694, 4668, 4643, 4618, 4593, 4569,
+       4545, 4521, 4498, 4474, 4451, 4429, 4406, 4384,
+       4362, 4338, 4314, 4291, 4267, 4244, 4222, 4199,
+       4177, 4155, 4134, 4112, 4091, 4070, 4050, 4029,
+       4009, 3989, 3969, 3950, 3931, 3911, 3893, 3874,
+       3855, 3837, 3819, 3801, 3784, 3766, 3749, 3732,
+       3715, 3698, 3681, 3665, 3649, 3633, 3617, 3601,
+       3585, 3570, 3554, 3539, 3524, 3509, 3495, 3480,
+       3466, 3451, 3437, 3423, 3409, 3395, 3382, 3368,
+       3355, 3342, 3328, 3315, 3302, 3290, 3277, 3264,
+       3252, 3240, 3227, 3215, 3203, 3191, 3179, 3168,
+       3156, 3144, 3133, 3122, 3110, 3099, 3088, 3077,
+       3066, 3056, 3045, 3034, 3024, 3013, 3003, 2993,
+       2982, 2972, 2962, 2952, 2943, 2933, 2923, 2913,
+       2904, 2894, 2885, 2876, 2866, 2857, 2848, 2839,
+       2830, 2821, 2812, 2803, 2795, 2786, 2778, 2769,
+       2761, 2752, 2744, 2736, 2727, 2719, 2711, 2703,
+       2695, 2687, 2679, 2671, 2664, 2656, 2648, 2641,
+       2633, 2626, 2618, 2611, 2603, 2596, 2589, 2581,
+       2574, 2566, 2559, 2552, 2544, 2537, 2530, 2523,
+       2515, 2508, 2501, 2494, 2487, 2480, 2474, 2467,
+       2460, 2453, 2447, 2440, 2433, 2427, 2420, 2414,
+       2408, 2401, 2395, 2389, 2382, 2376, 2370, 2364,
+       2358, 2352, 2346, 2340, 2334, 2328, 2322, 2316,
+       2311, 2305, 2299, 2293, 2288, 2282, 2277, 2271,
+       2266, 2260, 2255, 2249, 2244, 2239, 2233, 2228,
+       2223, 2218, 2212, 2207, 2202, 2197, 2192, 2187,
+       2182, 2177, 2172, 2167, 2162, 2157, 2153, 2148,
+       2143, 2138, 2134, 2129, 2124, 2120, 2115, 2110,
+       2106, 2101, 2097, 2092, 2088, 2083, 2079, 2075,
+       2070, 2066, 2062, 2057, 2053, 2049, 2045, 2040,
+       2036, 2032, 2028, 2024, 2020, 2016, 2012, 2007,
+       2003, 2000, 1996, 1992, 1988, 1984, 1980, 1976,
+       1972, 1968, 1965, 1961, 1957, 1953, 1949, 1946,
+       1942, 1938, 1935, 1931, 1927, 1924, 1920, 1917,
+       1913, 1910, 1906, 1903, 1899, 1896, 1892, 1889,
+       1885, 1882, 1879, 1875, 1872, 1869, 1865, 1862,
+       1859, 1855, 1852, 1849, 1846, 1842, 1839, 1836,
+       1833, 1830, 1827, 1823, 1820, 1817, 1814, 1811,
+       1808, 1805, 1802, 1799, 1796, 1793, 1790, 1787,
+       1784, 1781, 1778, 1775, 1773, 1770, 1767, 1764,
+       1761, 1758, 1755, 1753, 1750, 1747, 1744, 1742,
+       1739, 1736, 1733, 1731, 1728, 1725, 1723, 1720,
+       1717, 1715, 1712, 1709, 1707, 1704, 1702, 1699,
+       1696, 1694, 1691, 1689, 1686, 1684, 1681, 1679,
+       1676, 1674, 1671, 1669, 1666, 1664, 1662, 1659,
+       1657, 1654, 1652, 1650, 1647, 1645, 1643, 1640,
 };

diff --git a/projects/microb2010/tests/static_beacon/static_beacon.c b/projects/microb2010/tests/static_beacon/static_beacon.c
index ef6e1aa63b1de2a39c16aa7ed7ed76281c20e0b2..7f6b72c263c6bad0565ebee8552690b54e9c05bf 100755 (executable)
--- a/projects/microb2010/tests/static_beacon/static_beacon.c
+++ b/projects/microb2010/tests/static_beacon/static_beacon.c
@@ -41,12 +41,14 @@
 
 //#define NO_MODULATION
 #define WAIT_LASER
-#define MODUL_455KHZ
-//#define MODUL_56KHZ
+//#define MODUL_455KHZ
+#define MODUL_56KHZ
 //#define MODUL_38KHZ
-//#define SPEED_40RPS
+//#define INC_FRAME
+//#define FIXED_FRAME
+//#define SEND_RAWTIME
+#define LOCK_BUS
 #define SPEED_20RPS
-//#define SPEED_10RPS
 
 /* beacon identifier: must be odd, 3 bits */
 #define BEACON_ID 0x1
@@ -106,10 +108,15 @@
 #define IR_DDR  DDRB
 #define IR_BIT  1
 
+#define LOCKBUS_PORT PORTD
+#define LOCKBUS_PIN  PIND
+#define LOCKBUS_DDR  DDRD
+#define LOCKBUS_BIT  3
+
 /* FRAME must be odd */
 /* #define FRAME 0x0B /\* in little endian 1-1-0-1 *\/ */
 /* #define FRAME_LEN 4 */
-#define FRAME 0x4A5B /* in little endian */
+#define FRAME 0x621 /* in little endian */
 #define FRAME_LEN 16
 
 /* pin returns !0 when nothing, and 0 when laser is on photodiode */
@@ -117,25 +124,11 @@
 #define PHOTO_BIT 0
 #define READ_PHOTO() (!!(PHOTO_PIN & (_BV(PHOTO_BIT))))
 
-/* IR_DELAY **must** be < 32768 */
-#if (defined SPEED_10RPS)
-#define MIN_INTER_TIME    ((uint16_t)(160*2))  /* t~=160us dist=350cm */
-#define MAX_INTER_TIME    ((uint16_t)(8000*2)) /* t=8ms dist=10cm */
-#define IR_DELAY          ((uint16_t)(8000*2))
-#define INTER_LASER_TIME   30 /* in ms */
-#elif (defined SPEED_20RPS)
-#define MIN_INTER_TIME    ((uint16_t)(40*16))   /* t~=80us dist=350cm */
-#define MAX_INTER_TIME    ((uint16_t)(2000*16)) /* t=2ms dist=? >10cm */
-#define IR_DELAY          ((uint16_t)(2000*16))
-#define INTER_LASER_TIME   10 /* in ms */
-#elif (defined SPEED_40RPS)
-#define MIN_INTER_TIME    ((uint16_t)(40*16))  /* t~=40us dist=350cm */
-#define MAX_INTER_TIME    ((uint16_t)(2000*16)) /* t=2ms dist=10cm */
-#define IR_DELAY          ((uint16_t)(2000*16))
+/* IR_DELAY must be < 32768 */
+#define MIN_INTER_TIME    ((uint16_t)(100*16U))   /* t~=160us dist=400cm */
+#define MAX_INTER_TIME    ((uint16_t)(4000*16U)) /* t=4ms dist=16cm */
+#define IR_DELAY          ((uint16_t)(MAX_INTER_TIME/2))
 #define INTER_LASER_TIME   10 /* in ms */
-#else
-#error "speed not defined"
-#endif
 
 extern prog_uint16_t framedist_table[];
 /* basic functions to transmit on IR */
@@ -199,7 +192,6 @@ static void xmit_bits(uint32_t frame, uint8_t nbit)
        xmit_0();
 }
 
-#ifdef WAIT_LASER
 /* val is 12 bits. Return the 16 bits value that includes the 4 bits
  * cksum in MSB. */
 static uint16_t do_cksum(uint16_t val)
@@ -224,22 +216,33 @@ static uint16_t do_cksum(uint16_t val)
 static uint32_t get_frame(uint16_t laserdiff)
 {
        uint32_t frame = 0;
-       uint16_t frame_dist = 256;
-       uint16_t step, val;
+       uint16_t val, mid, min, max;
 
        /* for calibration, return the time */
-       if (0)
-               return laserdiff;
+#ifdef INC_FRAME
+       static uint16_t fr = 1;
+       fr += 2;
+       return fr;
+#endif
+#ifdef SEND_RAWTIME
+       return laserdiff | 1; /* frame must be odd */
+#endif
+
+       min = 0;
+       max = 511;
+       while (min != max) {
+               mid = (max + min + 1) / 2;
+               val = pgm_read_word(&framedist_table[mid]);
 
-       for (step = 128; step != 0; step /= 2) {
-               val = pgm_read_word(&framedist_table[frame_dist]);
                if (laserdiff > val)
-                       frame_dist -= step;
+                       max = mid - 1;
                else
-                       frame_dist += step;
+                       min = mid;
        }
 
-       frame |= ((uint32_t)(frame_dist & FRAME_DATA_MASK) << FRAME_DATA_SHIFT);
+       frame |= ((uint32_t)((min << FRAME_DATA_SHIFT)
+                            & FRAME_DATA_MASK));
+       frame |= BEACON_ID;
 
        /* process cksum and return */
        return do_cksum(frame);
@@ -254,17 +257,11 @@ static inline int8_t wait_laser(uint16_t *when, uint16_t *laserdiff)
        uint16_t time1, time2;
        uint16_t diff;
 
-#if (defined SPEED_40RPS) || (defined SPEED_20RPS)
        /* set timer to 16Mhz, we will use ICP */
        TCCR1A = 0;
        TCCR1B = _BV(CS10);
-#else /* 10 RPS */
-       /* set timer to 2Mhz, we will use ICP */
-       TCCR1A = 0;
-       TCCR1B = _BV(CS11);
-#endif
 
-       /* wait until all is off (inverted logic)  */
+       /* wait until all is off (inverted logic) */
        while (READ_PHOTO() == 0);
        TIFR = _BV(ICF1);
 
@@ -293,16 +290,20 @@ static inline int8_t wait_laser(uint16_t *when, uint16_t *laserdiff)
 
        /* process time difference */
        diff = time2 - time1;
-       if (diff < MIN_INTER_TIME)
+       if (diff > MAX_INTER_TIME)
                return -1;
 
+       /*
+        * diff/2 is < 32768
+        * ir_delay is < 32768
+        * current time is ~time1 + diff
+        */
        *when = time1 + (diff/2) + IR_DELAY;
        *laserdiff = diff;
 
        /* laser ok */
        return 0;
 }
-#endif
 
 /* */
 
@@ -310,11 +311,10 @@ int main(void)
 {
        /* must be odd */
        uint32_t frame = FRAME;
-#ifdef WAIT_LASER
        int8_t ret;
        uint16_t when = 0;
        uint16_t diff = 0;
-#endif
+
        /* LEDS */
        LED_DDR = _BV(LED1_BIT) | _BV(LED2_BIT) | _BV(LED3_BIT);
        IR_DDR |= _BV(IR_BIT);
@@ -377,10 +377,13 @@ int main(void)
        DDRB |= 0x08;
        OCR2 = 50;
        TCCR2 = _BV(WGM21) | _BV(WGM20) | _BV(COM21) | _BV(CS21) ;
-       
+
        while (1);
 #endif
 
+#ifdef LOCK_BUS
+       LOCKBUS_PORT &= (~_BV(LOCKBUS_BIT));
+#endif
 
        /* configure timer 0, prescaler = 64 */
        TCCR0 = _BV(CS01) | _BV(CS00);
@@ -395,17 +398,30 @@ int main(void)
 
                if (ret)
                        continue;
+#endif /* WAIT_LASER */
+
+#ifdef LOCK_BUS
+               if ((LOCKBUS_PIN & _BV(LOCKBUS_BIT)) == 0)
+                       continue;
+               LOCKBUS_DDR |= _BV(LOCKBUS_BIT);
+#endif
 
+#ifdef FIXED_FRAME
+               frame = FRAME;
+#else
                frame = get_frame(diff);
+#endif /* FIXED_FRAME */
                /* cannot convert into frame... skip it */
                if (frame == 0) {
                        wait_ms(INTER_LASER_TIME);
                        continue;
                }
 
+
+#ifdef WAIT_LASER
                /* wait before IR xmit */
                while ((int16_t)(when-TCNT1) > 0);
-#endif
+#endif /* WAIT_LASER */
 
                LED3_ON();
                /* ok, transmit frame */
@@ -413,6 +429,10 @@ int main(void)
 
                LED3_OFF();
 
+#ifdef LOCK_BUS
+               LOCKBUS_DDR &= (~_BV(LOCKBUS_BIT));
+#endif
+
                /* don't watch a laser during this time */
                wait_ms(INTER_LASER_TIME);
        }