projects/microb2010/tests/beacon_tsop/main.c

   1 /*
   2  *  Copyright Droids Corporation (2010)
   3  *  Olivier Matz <zer0@droids-corp.org>
   4  *
   5  *  This program is free software; you can redistribute it and/or modify
   6  *  it under the terms of the GNU General Public License as published by
   7  *  the Free Software Foundation; either version 2 of the License, or
   8  *  (at your option) any later version.
   9  *
  10  *  This program is distributed in the hope that it will be useful,
  11  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  *  GNU General Public License for more details.
  14  *
  15  *  You should have received a copy of the GNU General Public License
  16  *  along with this program; if not, write to the Free Software
  17  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  18  *
  19  *  Revision : $Id: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $
  20  *
  21  */
  22
  23 #include <math.h>
  24
  25 #include <aversive.h>
  26 #include <aversive/wait.h>
  27
  28 #include <uart.h>
  29 #include <pid.h>
  30 #include <pwm_ng.h>
  31 #include <parse.h>
  32 #include <rdline.h>
  33 #include <vect_base.h>
  34 #include <lines.h>
  35 #include <circles.h>
  36
  37 #include "cmdline.h"
  38 #include "uart_proto.h"
  39 #include "trigo.h"
  40 #include "main.h"
  41 #include "i2c_commands.h"
  42
  43 #define BOARD2010
  44 //#define BOARD2006
  45
  46 #ifdef BOARD2010
  47 #include "board2010.h"
  48 #else
  49 #include "board2006.h"
  50 #endif
  51
  52 /******************* TSOP */
  53
  54 struct detected_frame {
  55         uint16_t frame;
  56         uint16_t ref_time;
  57         uint16_t time;
  58         uint16_t tick;
  59 };
  60
  61 /* frame */
  62 struct frame_status {
  63         uint8_t led_cpt;
  64         uint16_t ref_time;
  65         uint16_t start_time;
  66         uint16_t frame;
  67         uint16_t mask;
  68         uint16_t prev_time;
  69         uint16_t time_long;
  70         uint16_t time_short;
  71         uint8_t prev_tsop;
  72         uint8_t len;
  73         uint8_t frame_len;
  74         uint8_t val;
  75 #define FRAME_RING_ORDER 4
  76 #define FRAME_RING_SIZE  (1<<FRAME_RING_ORDER)
  77 #define FRAME_RING_MASK  (FRAME_RING_SIZE-1)
  78         uint8_t head;
  79         uint8_t tail;
  80         struct detected_frame ring[FRAME_RING_SIZE];
  81 };
  82
  83 static struct frame_status static_beacon;
  84 static struct frame_status opp_beacon;
  85 static uint16_t tick = 0;
  86
  87 #define MIN_DIST 200.
  88 #define MAX_DIST 3500.
  89
  90 /* in ticks (=CS_PERIOD), age before the entry is removed from ring */
  91 #define MAX_CAP_AGE 30
  92
  93 /********************** CS */
  94
  95 /* 8ms, easier if it's a pow of 2 */
  96 #define CS_PERIOD_US (8192)
  97 #define CS_PERIOD ((uint16_t)(CS_PERIOD_US/4))
  98 #define CPT_ICR_MAX (uint8_t)((1000000UL/(uint32_t)CS_PERIOD_US)) /* too slow = 1 tr/s */
  99 #define CPT_ICR_MIN (uint8_t)((10000UL/(uint32_t)CS_PERIOD_US))   /* too fast = 100 tr/s */
 100
 101 /* in tr / 1000s */
 102 #define CS_CONSIGN (20 * 1000L)
 103
 104 /* 5% tolerance to validate captures, period is in  */
 105 #define TIM3_UNIT 250000000L
 106 #define MOTOR_PERIOD_MIN ((uint32_t)((250000000L/CS_CONSIGN) * 0.95))
 107 #define MOTOR_PERIOD_MAX ((uint32_t)((250000000L/CS_CONSIGN) * 1.05))
 108
 109 /* pwm for laser:
 110  *  - clear on timer compare (CTC)
 111  *  - Toggle OC0 on compare match
 112  *  - prescaler = 1 */
 113 #define LASER_ON() do { TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); } while (0)
 114 #define LASER_OFF() do { TCCR0 = 0; } while (0)
 115
 116 struct beacon_tsop beacon_tsop;
 117 uint32_t cs_consign = CS_CONSIGN;
 118
 119 static uint32_t current_motor_period;
 120
 121 void debug_serial(void)
 122 {
 123 #if 0
 124         while (1) {
 125                 int16_t c;
 126                 c = uart_recv_nowait(0);
 127                 if (c != -1)
 128                         printf("%c", (char)(c+1));
 129                 LED1_ON();
 130                 wait_ms(500);
 131                 LED1_OFF();
 132                 wait_ms(500);
 133         }
 134 #endif
 135 }
 136
 137 void debug_tsop(void)
 138 {
 139 #if 0
 140         while (1) {
 141                 if (TSOP_READ())
 142                         LED1_OFF();
 143                 else {
 144                         LED1_ON();
 145                         wait_ms(500);
 146                 }
 147         }
 148 #endif
 149 }
 150
 151 /* val is 16 bits, including 4 bits-cksum in MSB, return 0xFFFF is
 152  * cksum is wrong, or the 12 bits value on success. */
 153 static uint16_t verify_cksum(uint16_t val)
 154 {
 155         uint16_t x, cksum;
 156
 157         x = (val & 0xfff);
 158         /* add the four 4-bits blocks of val together */
 159         cksum = val & 0xf;
 160         val = val >> 4;
 161         cksum += val & 0xf;
 162         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 163         val = val >> 4;
 164         cksum += val & 0xf;
 165         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 166         val = val >> 4;
 167         cksum += val & 0xf;
 168         cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
 169         if (cksum == 0xf)
 170                 return x;
 171         return 0xffff; /* wrong cksum */
 172 }
 173
 174 static inline void decode_frame(struct frame_status *status,
 175                                 uint16_t ref_time, uint16_t cur_time, uint8_t cur_tsop)
 176 {
 177         uint16_t diff_time = cur_time - status->prev_time;
 178
 179         /* first rising edge */
 180         if (status->len == 0 && cur_tsop && diff_time > status->time_long) {
 181                 status->len = 1;
 182                 status->val = 1;
 183                 status->frame = 0;
 184                 status->start_time = cur_time;
 185                 status->ref_time = ref_time;
 186                 status->mask = 1;
 187         }
 188         /* any short pulse */
 189         else if (status->len != 0 && diff_time < status->time_short) {
 190                 if (status->len & 1) {
 191                         if (status->val)
 192                                 status->frame |= status->mask;
 193                         status->mask <<= 1;
 194                 }
 195                 status->len ++;
 196         }
 197         /* any long pulse */
 198         else if (status->len != 0 && diff_time < status->time_long) {
 199                 status->val = !status->val;
 200                 if (status->val)
 201                         status->frame |= status->mask;
 202                 status->mask <<= 1;
 203                 status->len += 2;
 204         }
 205         /* error case, reset */
 206         else {
 207                 status->len = 0;
 208         }
 209
 210         /* end of frame */
 211         if (status->len == status->frame_len*2) {
 212                 uint8_t tail_next = (status->tail+1) & FRAME_RING_MASK;
 213                 uint16_t frame_mask;
 214
 215                 frame_mask = (1 << status->frame_len) - 1;
 216
 217                 if (tail_next != status->head) {
 218                         status->ring[status->tail].frame = (status->frame & frame_mask);
 219                         status->ring[status->tail].ref_time = status->ref_time;
 220                         status->ring[status->tail].time = status->start_time;
 221                         status->ring[status->tail].tick = tick;
 222                         status->tail = tail_next;
 223                         status->led_cpt ++;
 224                 }
 225                 status->len = 0;
 226         }
 227
 228         status->prev_time = cur_time;
 229         status->prev_tsop = cur_tsop;
 230 }
 231
 232 /* decode frame */
 233 SIGNAL(SIG_TSOP_STA) {
 234         static uint8_t running = 0;
 235
 236         /* tsop status */
 237         uint8_t cur_tsop;
 238         uint16_t ref_time;
 239         uint16_t cur_time;
 240
 241         ref_time = ICR3;
 242         cur_time = TCNT3;
 243         cur_tsop = TSOP_STA_READ();
 244
 245         /* avoid interruption stacking */
 246         if (running)
 247                 return;
 248         running = 1;
 249         sei();
 250
 251         if (cur_tsop)
 252                 LED5_ON();
 253         else
 254                 LED5_OFF();
 255
 256         decode_frame(&static_beacon, ref_time, cur_time, cur_tsop);
 257
 258         running = 0;
 259 }
 260
 261 /* decode frame */
 262 SIGNAL(SIG_TSOP_OPP) {
 263         static uint8_t running = 0;
 264
 265         /* tsop status */
 266         uint8_t cur_tsop;
 267         uint16_t ref_time;
 268         uint16_t cur_time;
 269
 270         ref_time = ICR3;
 271         cur_time = TCNT3;
 272         cur_tsop = TSOP_OPP_READ();
 273
 274         /* avoid interruption stacking */
 275         if (running)
 276                 return;
 277         running = 1;
 278         sei();
 279
 280         if (cur_tsop)
 281                 LED6_ON();
 282         else
 283                 LED6_OFF();
 284
 285         decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop);
 286
 287         running = 0;
 288 }
 289
 290 /* absolute value */
 291 static inline int32_t AbS(int32_t x)
 292 {
 293         if (x > 0)
 294                 return x;
 295         else
 296                 return -x;
 297 }
 298
 299 /* Get the speed of motor (tr / 1000s)
 300  * - icr_cpt is the number of CS period between 2 ICR updates
 301  * - icr_diff is the difference of ICR values between the ICR updates
 302  *   (modulo 65536 obviously) */
 303 static inline int32_t get_speed(uint8_t icr_cpt, uint16_t icr_diff)
 304 {
 305         /* too slow (less than 1 tr/s) */
 306         if (icr_cpt >= CPT_ICR_MAX) {
 307                 current_motor_period = 250000;
 308                 return 1000L;
 309         }
 310
 311         /* too fast (more than 100 tr/s) */
 312         if (icr_cpt <= CPT_ICR_MIN) {
 313                 current_motor_period = 2500;
 314                 return 100000L;
 315         }
 316
 317         current_motor_period = icr_diff;
 318         return TIM3_UNIT/icr_diff;
 319 }
 320
 321 static int8_t check_sta_frame(uint16_t frame, uint16_t time)
 322 {
 323         int8_t beacon_id;
 324         uint16_t cksum;
 325
 326         /* ignore bad cksum */
 327         cksum = verify_cksum(frame);
 328         if (cksum == 0xFFFF)
 329                 goto fail;
 330
 331         beacon_id = frame & TSOP_STA_BEACON_ID_MASK;
 332
 333         if (beacon_id != TSOP_STA_BEACON_ID0 &&
 334             beacon_id != TSOP_STA_BEACON_ID1)
 335                 goto fail;
 336
 337         /* if motor speed is not good, skip values  */
 338         if (current_motor_period < MOTOR_PERIOD_MIN)
 339                 goto fail;
 340         if (current_motor_period > MOTOR_PERIOD_MAX)
 341                 goto fail;
 342
 343         return beacon_id;
 344
 345  fail:
 346         /* display if needed */
 347         if (beacon_tsop.debug_frame) {
 348                 printf("STA ID=%d frame=%x time=%d, cksum=%x\r\n",
 349                        beacon_id, frame, time, cksum);
 350         }
 351         return -1;
 352 }
 353
 354
 355 /* process the received frame ring */
 356 static void process_sta_ring(struct frame_status *status)
 357 {
 358         uint8_t head, head_next;
 359         uint16_t frame, frametick;
 360         uint8_t found = 0;
 361         int8_t beacon_id;
 362
 363         /* beacon 0 */
 364         uint16_t data0, time0, ref_time0;
 365         double angle0;
 366         double dist0;
 367
 368         /* beacon 1 */
 369         uint16_t data1, time1, ref_time1;
 370         double angle1;
 371         double dist1;
 372
 373         point_t pos;
 374         double a;
 375
 376         /* remove too old captures from the ring */
 377         while (status->head != status->tail) {
 378                 head_next = (status->head+1) & FRAME_RING_MASK;
 379                 frametick = status->ring[status->head].tick;
 380                 if ((uint16_t)(tick - frametick) < MAX_CAP_AGE)
 381                         break;
 382                 status->head = head_next;
 383         }
 384
 385         head = status->head;
 386         /* after CS, check if we have a new frame in ring */
 387         while (head != status->tail) {
 388                 head_next = (head+1) & FRAME_RING_MASK;
 389                 frame = status->ring[head].frame;
 390
 391                 beacon_id = check_sta_frame(frame, status->ring[head].time);
 392                 if (beacon_id < 0) {
 393                         head = head_next;
 394                         continue;
 395                 }
 396
 397                 if (beacon_id == TSOP_STA_BEACON_ID0) {
 398                         found |= 0x1;
 399                         data0 = (frame & TSOP_STA_FRAME_DATA_MASK) >> TSOP_STA_FRAME_DATA_SHIFT;
 400                         time0 = status->ring[head].time;
 401                         ref_time0 = status->ring[head].ref_time;
 402                 }
 403                 else if (beacon_id == TSOP_STA_BEACON_ID1) {
 404                         found |= 0x2;
 405                         data1 = (frame & TSOP_STA_FRAME_DATA_MASK) >> TSOP_STA_FRAME_DATA_SHIFT;
 406                         time1 = status->ring[head].time;
 407                         ref_time1 = status->ring[head].ref_time;
 408                 }
 409
 410                 head = head_next;
 411         }
 412
 413         /* if we didn't found beacon 0 and 1, return */
 414         if (found != 0x3)
 415                 return;
 416
 417         /* update ring head */
 418         status->head = head;
 419
 420         /* beacon 0 */
 421         dist0 = data0;
 422         dist0 /= 512.;
 423         dist0 *= (MAX_DIST-MIN_DIST);
 424         dist0 += MIN_DIST;
 425
 426         time0 = time0 - ref_time0;
 427         angle0 = (double)time0 / (double)current_motor_period;
 428         if (angle0 > 1.)
 429                 angle0 -= 1.;
 430         if (angle0 > 1.)
 431                 return; /* fail */
 432         angle0 *= (2 * M_PI);
 433         if (angle0 > M_PI)
 434                 angle0 -= M_PI;
 435
 436         /* beacon 1 */
 437         dist1 = data1;
 438         dist1 /= 512.;
 439         dist1 *= (MAX_DIST-MIN_DIST);
 440         dist1 += MIN_DIST;
 441
 442         time1 = time1 - ref_time1;
 443         angle1 = (double)time1 / (double)current_motor_period;
 444         if (angle1 > 1.)
 445                 angle1 -= 1.;
 446         if (angle1 > 1.)
 447                 return; /* fail */
 448         angle1 *= (2 * M_PI);
 449         if (angle0 > M_PI)
 450                 angle0 -= M_PI;
 451
 452         /* display if needed */
 453         if (beacon_tsop.debug_frame) {
 454                 printf("STA ID=%d dist0=%2.2f angle0=%2.2f dist1=%2.2f angle1=%2.2f\r\n",
 455                        beacon_id, dist0, angle0 * 180. / M_PI, dist1, angle1 * 180. / M_PI);
 456         }
 457
 458         if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon2, angle0, dist0,
 459                          angle1, dist1) < 0)
 460                 return;
 461
 462         /* /!\ angle is between 0 and 3600 */
 463         xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)(a*10));
 464 }
 465
 466 static int8_t check_opp_frame(uint16_t frame, uint16_t time)
 467 {
 468         int8_t beacon_id = -1;
 469         uint16_t cksum;
 470
 471         /* ignore bad cksum */
 472         cksum = verify_cksum(frame);
 473         if (cksum == 0xFFFF)
 474                 goto fail;
 475
 476         beacon_id = frame & TSOP_OPP_BEACON_ID_MASK;
 477         if (beacon_id != TSOP_OPP_BEACON_ID)
 478                 goto fail;
 479
 480         /* if motor speed is not good, skip values  */
 481         if (current_motor_period < MOTOR_PERIOD_MIN)
 482                 goto fail;
 483         if (current_motor_period > MOTOR_PERIOD_MAX)
 484                 goto fail;
 485
 486         return beacon_id;
 487  fail:
 488         /* display if needed */
 489         if (beacon_tsop.debug_frame) {
 490                 printf("OPP ID=%d frame=%x time=%d cksum=%x d=%d\r\n",
 491                        beacon_id, frame, time, cksum,
 492                        (frame & TSOP_OPP_FRAME_DATA_MASK) >>
 493                        TSOP_OPP_FRAME_DATA_SHIFT);
 494         }
 495         return -1;
 496 }
 497
 498 /* process the received frame ring */
 499 static void process_opp_ring(struct frame_status *status)
 500 {
 501         uint8_t head_next;
 502         uint16_t frame;
 503         uint8_t found = 0;
 504         uint16_t data, time, ref_time;
 505         double angle;
 506         double dist;
 507
 508         /* after CS, check if we have a new frame in ring */
 509         while (status->head != status->tail) {
 510                 head_next = (status->head+1) & FRAME_RING_MASK;
 511                 frame = status->ring[status->head].frame;
 512
 513                 if (check_opp_frame(frame, status->ring[status->head].time) < 0) {
 514                         status->head = head_next;
 515                         continue;
 516                 }
 517
 518                 found = 1;
 519                 data = (frame & TSOP_OPP_FRAME_DATA_MASK) >> TSOP_OPP_FRAME_DATA_SHIFT;
 520                 time = status->ring[status->head].time;
 521                 ref_time = status->ring[status->head].ref_time;
 522
 523                 status->head = head_next;
 524         }
 525
 526         if (found == 0)
 527                 return;
 528
 529         dist = data;
 530         dist /= 512.;
 531         dist *= (MAX_DIST-MIN_DIST);
 532         dist += MIN_DIST;
 533
 534         time = time - ref_time;
 535         angle = (double)time / (double)current_motor_period;
 536         if (angle > 1.)
 537                 angle -= 1.;
 538         if (angle > 1.)
 539                 return; /* fail */
 540         angle *= 3600; /* angle in 1/10 deg */
 541
 542         /* display if needed */
 543         if (beacon_tsop.debug_frame) {
 544                 printf("OPP dist=%2.2f angle=%2.2f\r\n", dist, angle/10);
 545         }
 546         xmit_opp((uint16_t)dist, (uint16_t)angle);
 547 }
 548
 549 int main(void)
 550 {
 551         uint16_t prev_cs = 0;
 552         uint16_t prev_icr = 0;
 553         uint16_t icr = 0;
 554         uint16_t diff_icr = 0;
 555         uint8_t cpt_icr = 0;
 556         uint8_t cpt = 0;
 557         int32_t speed = 0, out, err;
 558         uint16_t tcnt3;
 559         int16_t c;
 560         uint8_t x = 0; /* debug display counter */
 561
 562         opp_beacon.frame_len = TSOP_OPP_FRAME_LEN;
 563         opp_beacon.time_long = TSOP_OPP_TIME_LONG;
 564         opp_beacon.time_short = TSOP_OPP_TIME_SHORT;
 565
 566         static_beacon.frame_len = TSOP_STA_FRAME_LEN;
 567         static_beacon.time_long = TSOP_STA_TIME_LONG;
 568         static_beacon.time_short = TSOP_STA_TIME_SHORT;
 569
 570         /* LEDS */
 571         LED_DDR_INIT();
 572         DDRB |= 0x10; /* OC0 (laser pwm) */
 573
 574         /* PID init */
 575         pid_init(&beacon_tsop.pid);
 576         pid_set_gains(&beacon_tsop.pid, 700, 10, 0);
 577         pid_set_maximums(&beacon_tsop.pid, 0, 200000, 4095);
 578         pid_set_out_shift(&beacon_tsop.pid, 10);
 579         pid_set_derivate_filter(&beacon_tsop.pid, 4);
 580
 581         uart_init();
 582 #if CMDLINE_UART == 0
 583         fdevopen(uart0_dev_send, uart0_dev_recv);
 584 #elif CMDLINE_UART == 1
 585         fdevopen(uart1_dev_send, uart1_dev_recv);
 586 #endif
 587
 588         rdline_init(&beacon_tsop.rdl, write_char, valid_buffer, complete_buffer);
 589         snprintf(beacon_tsop.prompt, sizeof(beacon_tsop.prompt), "beacon > ");
 590         rdline_newline(&beacon_tsop.rdl, beacon_tsop.prompt);
 591
 592         debug_tsop();
 593         debug_serial();
 594
 595         /* configure external interrupt for TSOP */
 596         EICRx_TSOP |= _BV(ISCx0_TSOP_STA) | _BV(ISCx0_TSOP_OPP);
 597         EIMSK |= _BV(INTx_TSOP_STA) | _BV(INTx_TSOP_OPP);
 598
 599         /* pwm for motor */
 600         PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
 601                                  TIMER1_PRESCALER_DIV_1);
 602 #ifdef BOARD2010
 603         PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, C, 10, 0, NULL, 0);
 604 #else
 605         PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, A, 10, 0, NULL, 0);
 606 #endif
 607
 608         /* pwm for laser:
 609          *  - clear on timer compare (CTC)
 610          *  - Toggle OC0 on compare match
 611          *  - prescaler = 1 */
 612         TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
 613         OCR0 = 18; /* f ~= 420 khz at 16 Mhz */
 614
 615         /* configure timer 3: CLK/64
 616          * it is used as a reference time
 617          * enable noise canceller for ICP3 */
 618         TCCR3B = _BV(CS11) | _BV(CS10);
 619
 620         sei();
 621
 622         /* Control system will be done in main loop */
 623         while (1) {
 624
 625                 /* process pending bytes on uart */
 626                 cmdline_process();
 627
 628                 /* monitor the value of ICR (which is modified
 629                  * automatically on TT rising edge). If the value
 630                  * changed, process the time difference. */
 631                 if (ETIFR & _BV(ICF3)) {
 632                         cli();
 633                         icr = ICR3;
 634                         sei();
 635                         ETIFR = _BV(ICF3);
 636
 637                         LED2_TOGGLE();
 638                         diff_icr = (icr - prev_icr);
 639                         cpt_icr = cpt;
 640                         prev_icr = icr;
 641                         cpt = 0;
 642                         speed = get_speed(cpt_icr, diff_icr);
 643                 }
 644
 645                 /* read time reference */
 646                 cli();
 647                 tcnt3 = TCNT3;
 648                 sei();
 649
 650                 /* wait cs period */
 651                 if (tcnt3 - prev_cs < CS_PERIOD)
 652                         continue;
 653
 654                 /* CS LED */
 655                 if (x & 0x80)
 656                         LED1_ON();
 657                 else
 658                         LED1_OFF();
 659                 x++;
 660
 661                 /* process CS... maybe we don't need to use
 662                  * control_system_manager, just PID is enough */
 663
 664                 if (cpt == CPT_ICR_MAX)
 665                         speed = 0;
 666
 667                 /* enabled laser when rotation speed if at least 5tr/s */
 668                 if (1 || speed > 5000) /* XXX */
 669                         LASER_ON();
 670                 else
 671                         LASER_OFF();
 672
 673                 err = cs_consign - speed;
 674                 out = pid_do_filter(&beacon_tsop.pid, err);
 675                 if (out < 0)
 676                         out = 0;
 677                 if (out > 3000)
 678                         out = 3000;
 679
 680                 if (x == 0 && beacon_tsop.debug_speed)
 681                         printf("%ld %ld %u %u / %u\r\n",
 682                                speed, out, diff_icr, cpt_icr, cpt);
 683
 684                 pwm_ng_set(&beacon_tsop.pwm_motor, out);
 685
 686                 prev_cs = tcnt3;
 687
 688                 /* count the number of CS period between 2 ICR
 689                  * captures */
 690                 if (cpt < CPT_ICR_MAX)
 691                         cpt ++;
 692
 693                 process_sta_ring(&static_beacon);
 694                 process_opp_ring(&opp_beacon);
 695
 696                 c = uart_proto_recv();
 697                 if (c == I2C_COLOR_YELLOW) {
 698                         beacon0.x = -70;
 699                         beacon0.y = 1050;
 700                         beacon1.x = 3065;
 701                         beacon1.y = -65;
 702                         beacon2.x = 3065;
 703                         beacon2.y = 2165;
 704                 }
 705                 else {
 706                         beacon0.x = -70;
 707                         beacon0.y = 1050;
 708                         beacon1.x = 3065;
 709                         beacon1.y = 2165;
 710                         beacon2.x = 3065;
 711                         beacon2.y = -65;
 712                 }
 713
 714                 cli();
 715                 tick ++; /* global imprecise time reference */
 716                 sei();
 717         }
 718
 719         return 0;
 720 }