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