2 * Copyright Droids Corporation (2010)
3 * Olivier Matz <zer0@droids-corp.org>
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.
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.
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
19 * Revision : $Id: main.c,v 1.8 2009-05-02 10:08:09 zer0 Exp $
26 #include <aversive/wait.h>
33 #include <vect_base.h>
38 #include "uart_proto.h"
46 #include "board2010.h"
48 #include "board2006.h"
51 /******************* TSOP */
53 struct detected_frame {
74 #define FRAME_RING_ORDER 4
75 #define FRAME_RING_SIZE (1<<FRAME_RING_ORDER)
76 #define FRAME_RING_MASK (FRAME_RING_SIZE-1)
79 struct detected_frame ring[FRAME_RING_SIZE];
82 static struct frame_status static_beacon;
83 static struct frame_status opp_beacon;
84 static uint16_t tick = 0;
87 #define MAX_DIST 3500.
89 /* in ticks (=CS_PERIOD), age before the entry is removed from ring */
90 #define MAX_CAP_AGE 30
92 /********************** CS */
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 */
101 #define CS_CONSIGN (15 * 1000L)
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))
109 * - clear on timer compare (CTC)
110 * - Toggle OC0 on compare match
112 #define LASER_ON() do { TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00); } while (0)
113 #define LASER_OFF() do { TCCR0 = 0; } while (0)
115 struct beacon_tsop beacon_tsop;
116 uint32_t cs_consign = CS_CONSIGN;
118 static uint32_t current_motor_period;
120 void debug_serial(void)
125 c = uart_recv_nowait(0);
127 printf("%c", (char)(c+1));
136 void debug_tsop(void)
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)
157 /* add the four 4-bits blocks of val together */
161 cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
164 cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
167 cksum = (cksum & 0xf) + ((cksum & 0xf0) >> 4);
170 return 0xffff; /* wrong cksum */
173 static inline void decode_frame(struct frame_status *status,
174 uint16_t ref_time, uint16_t cur_time, uint8_t cur_tsop)
176 uint16_t diff_time = cur_time - status->prev_time;
178 /* first rising edge */
179 if (status->len == 0 && cur_tsop && diff_time > status->time_long) {
184 status->start_time = cur_time;
185 status->ref_time = ref_time;
188 /* any short pulse */
189 else if (status->len != 0 && diff_time < status->time_short) {
190 if (status->len & 1) {
192 status->frame |= status->mask;
198 else if (status->len != 0 && diff_time < status->time_long) {
199 status->val = !status->val;
201 status->frame |= status->mask;
205 /* error case, reset */
211 if (status->len == status->frame_len*2) {
212 uint8_t tail_next = (status->tail+1) & FRAME_RING_MASK;
215 frame_mask = (1 << status->frame_len) - 1;
217 if (tail_next != status->head) {
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) */
231 status->prev_time = cur_time;
232 status->prev_tsop = cur_tsop;
240 SIGNAL(SIG_TSOP_STA) {
241 static uint8_t running = 0;
250 cur_tsop = TSOP_STA_READ();
252 /* avoid interruption stacking */
263 decode_frame(&static_beacon, ref_time, cur_time, cur_tsop);
269 SIGNAL(SIG_TSOP_OPP) {
270 static uint8_t running = 0;
279 cur_tsop = TSOP_OPP_READ();
281 /* avoid interruption stacking */
292 //decode_frame(&opp_beacon, ref_time, cur_time, cur_tsop);
298 static inline int32_t AbS(int32_t x)
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)
313 int32_t best_diff = 65536L;
314 int8_t best_cpt = -2;
318 /* too slow (less than 1 tr/s) */
319 if (icr_cpt > CPT_ICR_MAX)
322 /* too fast (more than 100 tr/s) */
323 if (icr_cpt < CPT_ICR_MIN)
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);
336 if (AbS(diff) < AbS(best_diff)) {
338 best_cpt = icr_cpt + i;
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 */
347 /* too slow (less than 1 tr/s) */
348 if (icr_cpt >= CPT_ICR_MAX)
351 /* too fast (more than 100 tr/s) */
352 if (icr_cpt <= CPT_ICR_MIN)
357 return icr_cpt * 8192UL;
359 return TIM3_UNIT/icr_diff;
362 static int8_t check_sta_frame(uint16_t frame, uint16_t time)
366 /* ignore bad cksum */
367 if (verify_cksum(frame) == 0xFFFF)
370 beacon_id = (frame >> TSOP_STA_BEACON_ID_SHIFT) & TSOP_STA_BEACON_ID_MASK;
372 if (beacon_id != TSOP_STA_BEACON_ID0 &&
373 beacon_id != TSOP_STA_BEACON_ID1)
376 /* if motor speed is not good, skip values */
377 if (current_motor_period < MOTOR_PERIOD_MIN)
379 if (current_motor_period > MOTOR_PERIOD_MAX)
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);
394 /* process the received frame ring */
395 static void process_sta_ring(struct frame_status *status)
397 uint8_t head, head_next;
398 uint16_t frame, frametick;
403 uint16_t data0, time0, ref_time0;
408 uint16_t data1, time1, ref_time1;
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)
421 status->head = head_next;
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;
430 beacon_id = check_sta_frame(frame, status->ring[head].time);
436 if (beacon_id == TSOP_STA_BEACON_ID0) {
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;
442 else if (beacon_id == TSOP_STA_BEACON_ID1) {
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;
452 /* if we didn't found beacon 0 and 1, return */
456 /* update ring head */
462 dist0 *= (MAX_DIST-MIN_DIST);
465 time0 = time0 - ref_time0;
466 angle0 = (double)time0 / (double)current_motor_period;
471 angle0 *= (2 * M_PI);
478 dist1 *= (MAX_DIST-MIN_DIST);
481 time1 = time1 - ref_time1;
482 angle1 = (double)time1 / (double)current_motor_period;
487 angle1 *= (2 * M_PI);
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);
497 if (ad_to_posxya(&pos, &a, 0, &beacon0, &beacon1, angle0, dist0,
501 xmit_static((uint16_t)pos.x, (uint16_t)pos.y, (uint16_t)a);
504 static int8_t check_opp_frame(uint16_t frame, uint16_t time)
506 int8_t beacon_id = -1;
508 /* ignore bad cksum */
509 if (verify_cksum(frame) == 0xFFFF)
512 beacon_id = (frame >> TSOP_OPP_BEACON_ID_SHIFT) & TSOP_OPP_BEACON_ID_MASK;
513 if (beacon_id != TSOP_OPP_BEACON_ID)
516 /* if motor speed is not good, skip values */
517 if (current_motor_period < MOTOR_PERIOD_MIN)
519 if (current_motor_period > MOTOR_PERIOD_MAX)
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);
532 /* process the received frame ring */
533 static void process_opp_ring(struct frame_status *status)
538 uint16_t data, time, ref_time;
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;
547 if (check_opp_frame(frame, status->ring[status->head].time) < 0) {
548 status->head = head_next;
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;
557 status->head = head_next;
565 dist *= (MAX_DIST-MIN_DIST);
568 time = time - ref_time;
569 angle = (double)time / (double)current_motor_period;
574 angle *= 3600; /* angle in 1/10 deg */
576 /* display if needed */
577 if (beacon_tsop.debug_frame) {
578 printf("OPP dist=%2.2f angle=%2.2f\r\n", dist, angle/10);
580 xmit_opp((uint16_t)dist, (uint16_t)angle);
585 uint16_t prev_cs = 0;
586 uint16_t prev_icr = 0;
588 uint16_t diff_icr = 0;
591 int32_t speed = 0, out, err;
593 uint8_t x = 0; /* debug display counter */
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;
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;
605 DDRB |= 0x10; /* OC0 (laser pwm) */
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);
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);
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);
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);
633 PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
634 TIMER1_PRESCALER_DIV_1);
636 PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, C, 10, 0, NULL, 0);
638 PWM_NG_INIT16(&beacon_tsop.pwm_motor, 1, A, 10, 0, NULL, 0);
642 * - clear on timer compare (CTC)
643 * - Toggle OC0 on compare match
645 TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
646 OCR0 = 18; /* f ~= 420 khz at 16 Mhz */
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);
655 /* Control system will be done in main loop */
658 /* process pending bytes on uart */
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)) {
671 diff_icr = (icr - prev_icr);
675 speed = get_speed(cpt_icr, diff_icr);
678 /* read time reference */
684 if (tcnt3 - prev_cs < CS_PERIOD)
694 /* process CS... maybe we don't need to use
695 * control_system_manager, just PID is enough */
697 if (cpt == CPT_ICR_MAX)
700 /* enabled laser when rotation speed if at least 5tr/s */
701 if (1 || speed > 5000) /* XXX */
706 err = cs_consign - speed;
707 out = pid_do_filter(&beacon_tsop.pid, err);
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);
717 pwm_ng_set(&beacon_tsop.pwm_motor, out);
721 /* count the number of CS period between 2 ICR
723 if (cpt < CPT_ICR_MAX)
726 process_sta_ring(&static_beacon);
727 process_opp_ring(&opp_beacon);
729 tick ++; /* global imprecise time reference */