From 4e68c9b6f30c864b671b5c51affe61d7c9205465 Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 8 May 2010 14:57:35 +0200 Subject: [PATCH] static and tourel beacon --- .../microb2010/tests/beacon_tsop/board2006.h | 12 +- .../microb2010/tests/beacon_tsop/board2010.h | 14 +- projects/microb2010/tests/beacon_tsop/main.c | 118 ++++++---------- .../microb2010/tests/static_beacon/coding.py | 31 ++--- .../tests/static_beacon/framedist.c | 128 +++++++++--------- .../tests/static_beacon/static_beacon.c | 112 ++++++++------- 6 files changed, 197 insertions(+), 218 deletions(-) diff --git a/projects/microb2010/tests/beacon_tsop/board2006.h b/projects/microb2010/tests/beacon_tsop/board2006.h index 1b3f67c..841ab15 100644 --- 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 f66faa1..e558561 100644 --- 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 7d819bb..2edd572 100755 --- 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 - * + * * 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 9ef4c5c..05b20ab 100644 --- 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 " 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]))), - 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 eafe45d..ed7892a 100644 --- a/projects/microb2010/tests/static_beacon/framedist.c +++ b/projects/microb2010/tests/static_beacon/framedist.c @@ -1,68 +1,68 @@ #include #include 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 ef6e1aa..7f6b72c 100755 --- 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); } -- 2.39.5