projects/microb2009/sensorboard/scanner.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <math.h>
   5
   6 #include <aversive.h>
   7 #include <aversive/wait.h>
   8 #include <aversive/error.h>
   9 #include <aversive/pgmspace.h>
  10 #include <pwm_ng.h>
  11 #include <pid.h>
  12 #include <time.h>
  13 #include <quadramp.h>
  14 #include <control_system_manager.h>
  15 #include <adc.h>
  16 #include <spi.h>
  17 #include <ax12.h>
  18
  19 #include <time.h>
  20 #include <blocking_detection_manager.h>
  21
  22 #include <encoders_spi.h>
  23
  24 #include <rdline.h>
  25
  26 #include "sensor.h"
  27
  28 #include <uart.h>
  29
  30 #include "main.h"
  31 #include "scanner.h"
  32
  33 #include "cmdline.h"
  34
  35
  36 #include "scan_h_l.h"
  37
  38
  39 #include <vect_base.h>
  40 #include "img_processing.h"
  41
  42
  43 #include "../common/i2c_commands.h"
  44
  45 #define SCANNER_DEBUG(args...) DEBUG(E_USER_SCANNER, args)
  46 #define SCANNER_NOTICE(args...) NOTICE(E_USER_SCANNER, args)
  47 #define SCANNER_ERROR(args...) ERROR(E_USER_SCANNER, args)
  48
  49
  50 #define MODULO_TIMER (1023L)
  51
  52 #define COEFF_TIMER (2)
  53 #define COEFF_MULT (1000L)
  54 #define COEFF_MULT2 (1000L)
  55
  56 double  TELEMETRE_A =  TELEMETRE_A_INIT;
  57 double  TELEMETRE_B = TELEMETRE_B_INIT;
  58
  59
  60
  61
  62 struct scan_params scan_params;
  63
  64 static volatile int32_t scan_delta_pos;
  65 static volatile int32_t scan_tick_cur = 0;
  66 static volatile int32_t scan_tick_prev = 0;
  67 /*static volatile int32_t count = 0;
  68 static volatile int32_t count_diff_rising  = 0;
  69 static volatile int32_t count_diff_falling = 0;
  70 */
  71 static int32_t scanner_coeff = 0;
  72
  73 //static volatile int8_t valid_scanner = 0;
  74
  75 static volatile int32_t scan_pos_cur = 0;
  76
  77 static int32_t pos_ref = 0;
  78
  79
  80 int32_t encoders_spi_get_value_scanner(void *number)
  81 {
  82         int32_t ret;
  83
  84         ret = encoders_spi_get_value(number);
  85         return ret*4;
  86 }
  87
  88
  89 void encoders_spi_set_value_scanner(void * number, int32_t v)
  90 {
  91         encoders_spi_set_value(number, v/4);
  92 }
  93
  94 int32_t encoders_spi_update_scanner(void * number)
  95 {
  96         int32_t ret;
  97         uint8_t flags;
  98
  99         IRQ_LOCK(flags);
 100         ret = encoders_spi_get_value_scanner(number);
 101         scan_delta_pos = ret - scan_pos_cur;
 102         scan_pos_cur = ret;
 103         scan_tick_prev = scan_tick_cur;
 104         scan_tick_cur = TCNT3;
 105
 106         scanner_coeff = (scan_delta_pos * COEFF_MULT) /
 107           ((scan_tick_cur - scan_tick_prev  + MODULO_TIMER + 1) & MODULO_TIMER);
 108
 109         IRQ_UNLOCK(flags);
 110
 111
 112         return ret;
 113 }
 114
 115 int32_t encoders_spi_get_value_scanner_interpolation(void * number)
 116 {
 117         uint8_t flags;
 118         int32_t pos;
 119
 120         IRQ_LOCK(flags);
 121         pos = scan_pos_cur;
 122         pos += (scanner_coeff * ((TCNT3 - scan_tick_cur + MODULO_TIMER + 1)& MODULO_TIMER ))/
 123           COEFF_MULT;
 124
 125         IRQ_UNLOCK(flags);
 126
 127         return pos;
 128 }
 129
 130
 131 void scanner_reset_pos(void)
 132 {
 133         pwm_ng_set(SCANNER_PWM, 0);
 134         encoders_spi_set_value_scanner(SCANNER_ENCODER, 0);
 135 }
 136
 137 void scanner_init(void)
 138 {
 139
 140         scan_params.working = 0;
 141         scan_params.must_stop = 0;
 142
 143         scanner_reset_pos();
 144         pos_ref = encoders_spi_get_value_scanner(SCANNER_ENCODER);
 145
 146         //memset(&scanner, 0, sizeof(struct scanner));
 147
 148         scan_delta_pos = 0;
 149
 150         /*for(i=0;i<SCANNER_MAX_SAMPLE;i++)
 151                 scanner_sample_size[i] = 0;*/
 152
 153
 154
 155 }
 156
 157
 158 #define SCANNER_OFFSET_CALIBRE 1500
 159
 160 #define CALIBRE_LASER_SAMPLE 100
 161
 162
 163
 164
 165
 166
 167 void scanner_calibre_laser(void)
 168 {
 169         unsigned int i;
 170         int32_t laser_value = 0;
 171
 172         /* arm out */
 173         pwm_ng_set(&gen.servo3, SCANNER_POS_OUT);
 174
 175         /* set mirror to have vertical laser */
 176         cs_set_consign(&sensorboard.scanner.cs, (35L*SCANNER_STEP_TOUR)/100);
 177
 178         wait_ms(500);
 179
 180         /*
 181            laser must point on the ground:
 182            we sample laser in order to determine
 183            laser cold/warm calibration
 184         */
 185
 186         for (i = 0; i<CALIBRE_LASER_SAMPLE; i++){
 187                 laser_value +=  adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
 188                 wait_ms(2);
 189         }
 190
 191         laser_value/=CALIBRE_LASER_SAMPLE;
 192
 193         SCANNER_NOTICE("laser calibration value %"PRIi32"", laser_value);
 194
 195         quadramp_set_1st_order_vars(&sensorboard.scanner.qr, scan_params.speed, scan_params.speed); /* set speed */
 196         cs_set_consign(&sensorboard.scanner.cs, 0);
 197         wait_ms(200);
 198
 199
 200         TELEMETRE_B = TELEMETRE_B_INIT + ((double)(424 - laser_value))*6./90.;
 201
 202         SCANNER_NOTICE("TEL B value %2.2f", TELEMETRE_B);
 203
 204
 205         /* arm in */
 206
 207         pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
 208
 209
 210 }
 211
 212
 213
 214
 215
 216 void scanner_calibre_mirror(void)
 217 {
 218
 219         sensorboard.flags &= ~DO_CS;
 220
 221
 222
 223         /* set arm in calibre position */
 224         pwm_ng_set(&gen.servo3, SCANNER_POS_CALIBRE);
 225         wait_ms(500);
 226
 227
 228
 229         /* init scanner pos */
 230         pwm_ng_set(SCANNER_PWM, 100);
 231
 232         /* find rising edge of the mirror*/
 233         wait_ms(100);
 234         while (sensor_get(SCANNER_POS_SENSOR));
 235         wait_ms(100);
 236         while (!sensor_get(SCANNER_POS_SENSOR));
 237
 238         pwm_ng_set(SCANNER_PWM, 0);
 239
 240
 241         scanner_reset_pos();
 242         pid_reset(&sensorboard.scanner.pid);
 243         cs_set_consign(&sensorboard.scanner.cs, 0);
 244
 245
 246         quadramp_reset(&sensorboard.scanner.qr);
 247
 248
 249         sensorboard.flags |= DO_CS;
 250
 251
 252         /*
 253            set mirror to set laser point at maximum
 254            distance from robot
 255         */
 256         encoders_spi_set_value_scanner(SCANNER_ENCODER, -SCANNER_OFFSET_CALIBRE);
 257         wait_ms(100);
 258
 259         /* arm in */
 260
 261         pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
 262
 263 }
 264
 265
 266 /* arm must be in OUT position! */
 267 void scanner_do_scan(void){
 268         scan_params.working = 1;
 269         scan_params.dropzone_h = -1;
 270
 271
 272         quadramp_set_1st_order_vars(&sensorboard.scanner.qr, scan_params.speed, scan_params.speed); /* set speed */
 273
 274         scan_params.sample_i = MAX_SAMPLE;
 275         scan_params.pos_start_scan = encoders_spi_get_value_scanner(SCANNER_ENC);
 276
 277         memset(scan_params.sample_tab, 0xff, MAX_SAMPLE*sizeof(uint8_t));
 278
 279         cs_set_consign(&sensorboard.scanner.cs, scan_params.pos_start_scan+SCANNER_STEP_TOUR*200L);
 280
 281         scan_params.last_col_n = 0;
 282         scan_params.last_row_n = 0;
 283         scan_params.last_sample = 0;
 284
 285
 286 }
 287
 288
 289 void scanner_end_process(void)
 290 {
 291         int16_t dropzone_x, dropzone_y;
 292         int8_t ret;
 293         uint32_t t1, t2;
 294
 295         t1 = time_get_us2();
 296
 297
 298         SCANNER_NOTICE("process img algo %d", scan_params.algo);
 299         if (scan_params.algo == I2C_SCANNER_ALGO_COLUMN_DROPZONE) {
 300                 SCANNER_NOTICE("column dropzone h: %d x:%d y:%d", scan_params.drop_zone.working_zone,
 301                                scan_params.drop_zone.center_x, scan_params.drop_zone.center_y);
 302
 303                 ret = get_column_dropzone(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
 304                                           scan_params.drop_zone.working_zone, scan_params.drop_zone.center_x, scan_params.drop_zone.center_y,
 305                                           &dropzone_x, &dropzone_y);
 306
 307                 scan_params.dropzone_h = ret;
 308                 scan_params.dropzone_x = dropzone_x;
 309                 scan_params.dropzone_y = dropzone_y;
 310
 311                 SCANNER_NOTICE("best column h:%d x:%d y:%d",
 312                          scan_params.dropzone_h,
 313                          scan_params.dropzone_x, scan_params.dropzone_y);
 314
 315
 316         }
 317         else if (scan_params.algo == I2C_SCANNER_ALGO_CHECK_TEMPLE){
 318                 SCANNER_NOTICE("checktemple h: %d x:%d y:%d", scan_params.check_temple.level,
 319                                scan_params.check_temple.temple_x, scan_params.check_temple.temple_y);
 320                 ret = is_temple_there(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
 321                                       scan_params.check_temple.level, scan_params.check_temple.temple_x, scan_params.check_temple.temple_y);
 322
 323                 scan_params.dropzone_h = ret?scan_params.check_temple.level:-1;
 324         }
 325         else if (scan_params.algo == I2C_SCANNER_ALGO_TEMPLE_DROPZONE){
 326                 SCANNER_NOTICE("temple dropzone  h: %d x:%d y:%d", scan_params.drop_zone.working_zone,
 327                                scan_params.drop_zone.center_x, scan_params.drop_zone.center_y);
 328                 ret = find_temple_dropzone(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN,
 329                                            scan_params.drop_zone.working_zone, scan_params.drop_zone.center_x, scan_params.drop_zone.center_y,
 330                                            &dropzone_x, &dropzone_y);
 331
 332                 scan_params.dropzone_h = ret;
 333                 scan_params.dropzone_x = dropzone_x;
 334                 scan_params.dropzone_y = dropzone_y;
 335
 336                 SCANNER_NOTICE("best temple h:%d x:%d y:%d",
 337                          scan_params.dropzone_h,
 338                          scan_params.dropzone_x, scan_params.dropzone_y);
 339
 340         }
 341
 342         scan_params.working = 0;
 343
 344         t2 = time_get_us2();
 345         SCANNER_NOTICE("process total time %"PRIi32"",t2-t1);
 346
 347
 348 }
 349
 350
 351 void scanner_scan_autonomous(void)
 352 {
 353         /* arm out*/
 354         pwm_ng_set(&gen.servo3, SCANNER_POS_OUT);
 355         time_wait_ms(300);
 356
 357         scanner_do_scan();
 358
 359         while(scan_params.sample_i > 0){
 360                 time_wait_ms(10);
 361         }
 362
 363         /* arm in */
 364         pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
 365
 366 }
 367
 368
 369 /*
 370  * called from IRQ:
 371  * mode can be off/prepare/start, see in i2c_commands.h
 372  */
 373 void scanner_set_mode(uint8_t mode)
 374 {
 375         if (mode == I2C_SENSORBOARD_SCANNER_PREPARE){
 376                 /* reset flag max pos */
 377                 scan_params.max_column_detected = 0;
 378
 379                 /* arm out */
 380                 pwm_ng_set(&gen.servo3, SCANNER_POS_OUT);
 381
 382         }
 383         else if (mode == I2C_SENSORBOARD_SCANNER_STOP){
 384                 /* arm in */
 385                 pwm_ng_set(&gen.servo3, SCANNER_POS_IN);
 386                 scan_params.must_stop = 1;
 387         }
 388         else if (mode == I2C_SENSORBOARD_SCANNER_START){
 389                 scan_params.max_column_detected = 0;
 390                 scan_params.must_stop = 0;
 391
 392
 393                 /* start scan in background */
 394                 scanner_do_scan();
 395         }
 396
 397
 398 }
 399
 400 /*
 401 void scanner_stop(void)
 402 {
 403         sensorboard.scanner.on = 0;
 404         pwm_ng_set(SCANNER_PWM, 0);
 405 }
 406 */
 407
 408
 409 /*
 410 int32_t encoders_spi_get_scanner_speed(void * dummy)
 411 {
 412         return scanner_speed;
 413 }
 414 */
 415
 416
 417 //uint8_t sample_tab[MAX_SAMPLE];
 418 //uint16_t sample_i = 0;
 419
 420
 421 //#define offset_a (75.*M_PI/180.)
 422 //float offset_a;
 423 //float offset_b;
 424
 425 //int32_t pos_start_scan;
 426
 427
 428 /* get motor angle in radian; return mirror angle in radian, cos a sin a */
 429 void ang2_a_mirror(float b, float * c_a, float* s_a, float* a)
 430 {
 431         float x2, y2;
 432         float A, DELTA, B, D;
 433
 434         b+=scan_params.offset_b;
 435         x2 = X + l1*cos(b);
 436         y2 = Y + l1*sin(b);
 437
 438         A = (l3*l3 + x2*x2 + y2*y2 - l2*l2)/(2*l3);
 439
 440         DELTA = -(A*A - x2*x2 - y2*y2);
 441         B = sqrt(DELTA);
 442
 443         D = x2*x2 + y2*y2;
 444
 445         *c_a = (x2*A + y2*B)/D;
 446         *s_a = -(x2*B - y2*A)/D;
 447
 448         *a = atan2(*s_a, *c_a);
 449
 450         *a += scan_params.offset_a;
 451         // *s_a = sin(*a);
 452         // *c_a = cos(*a);
 453
 454 }
 455
 456 /* get telemeter dist , cos a, sin a, a and return H, L of scanned point */
 457 void ang2_H_L(float l_telemetre, float c_a, float s_a, float a, float *H, float *L)
 458 {
 459         float d;
 460         d = h_mirror*c_a/s_a;
 461         *H = (l_telemetre - l_mirror - d)*sin(2*a);
 462         *L = l_mirror + d + *H/tan(2*a);
 463
 464         //*H+= 8*sin(a-scan_params.offset_a);
 465 }
 466
 467
 468
 469 //int32_t last_col_n;
 470 //int32_t last_row_n;
 471 //uint8_t last_sample;
 472
 473 //uint8_t h_limit[] = {40, 53, 66, 78, 94, 111, 123};
 474 //uint8_t h_limit[] = {37, 48, 61, 72, 94, 111, 123};
 475
 476 /* last high is dummy, to deal higher columns */
 477 uint8_t h_limit[] = {68, 79, 93, 107, 121, 138, 155, 170, 250};
 478 #define H_MARGIN 7
 479
 480
 481 #if 0
 482 void do_scan(void * dummy)
 483 {
 484
 485         unsigned int i;
 486         int16_t a;
 487         int32_t row_n;
 488         int32_t col_n;
 489
 490
 491         int32_t tour_pos;
 492         int32_t pos, last_pos;
 493         int32_t pos_tmp ;
 494         int32_t mot_pos;
 495         float dist;
 496         uint8_t min_sample;
 497
 498         float b, c_a, s_a, H, L, m_a;
 499         int32_t H_fin;
 500
 501
 502         if (scan_params.sample_i==0)
 503                 return;
 504
 505         mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
 506
 507         if (scan_params.sample_i==1){
 508                 SCANNER_DEBUG("dump end enc %ld %d ", mot_pos, PIX_PER_SCAN);
 509                 //scanner.flags &= (~CS_ON);
 510
 511
 512
 513                 /* stop scan at cur pos + 10 round */
 514                 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
 515                 mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ;
 516
 517                 SCANNER_DEBUG("set to %ld ", mot_pos);
 518
 519                 cs_set_consign(&sensorboard.scanner.cs, mot_pos);
 520                 //pwm_ng_set(SCANNER_MOT_PWM, 0);
 521
 522
 523         }
 524
 525         mot_pos-= scan_params.pos_start_scan;
 526
 527         a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
 528
 529
 530         //dist = (a-TELEMETRE_B)/TELEMETRE_A;
 531         dist = TELEMETRE_A * a +TELEMETRE_B;
 532
 533         //SCANNER_DEBUG("enc val = %ld",        encoders_microb_get_value((void *)SCANNER_ENC));
 534
 535
 536         //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a;
 537         //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0;
 538
 539
 540         row_n = (mot_pos)/(SCANNER_STEP_TOUR/2);
 541 #if 0
 542         /* separe scan forward/backword */
 543         if (row_n%2){
 544                 row_n/=2;
 545         }
 546         else{
 547                 row_n/=2;
 548                 row_n+=30;
 549         }
 550 #endif
 551
 552         tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
 553
 554         b = (2.*M_PI)*(float)tour_pos/(float)(SCANNER_STEP_TOUR);
 555
 556         ang2_a_mirror(b, &c_a, &s_a, &m_a);
 557         ang2_H_L(dist, c_a, s_a, m_a, &H, &L);
 558
 559
 560         SCANNER_DEBUG("%ld %d", tour_pos, a);
 561
 562         if (H >0){
 563                 printf("zarb H\n");
 564                 H = 0;
 565         }
 566
 567         if (dist> SCAN_MAX_DIST){
 568                 H = 0;
 569                 L = 0;
 570         }
 571
 572         H = H;//m_a*180/M_PI;
 573         L = L;//(L-100)*PIX_PER_SCAN;
 574
 575         //SCANNER_DEBUG("polling : ADC0 = %i %f",a, dist);
 576         //SCANNER_DEBUG("%f %f  %2.2f %f", H, L, m_a*180./M_PI, dist);
 577
 578
 579         //SCANNER_DEBUG("%f %f", dist, m_a*180/M_PI);
 580
 581         H=(H+SCAN_H_MAX)*SCAN_H_COEF;
 582         L-=SCAN_L_MIN;
 583
 584
 585         /* XXX may never append */
 586         if (L<0)
 587                 L=0;
 588
 589
 590         /* first filter => pixel modulo level */
 591 #define H_BASE  10
 592 #define H_MUL  14
 593         H_fin = H;//+SCAN_H_MAX;
 594         //H_fin = ((H_fin-H_BASE)/H_MUL)*H_MUL + H_BASE;
 595
 596         if (scan_params.filter){
 597                 H_fin =  11; // default is level 11
 598                 for (i=0;i<sizeof(h_limit)/sizeof(uint8_t);i++){
 599                         if (H < h_limit[i]-H_MARGIN){
 600                                 H_fin = i;
 601                                 break;
 602                         }
 603                 }
 604         }
 605
 606         //SCANNER_DEBUG("%f %f", dist, m_a*180/M_PI);
 607         //SCANNER_DEBUG("%f %f", m_a*180/M_PI, b*180/M_PI);
 608
 609         //SCANNER_DEBUG("%d %f", a, b*180/M_PI);
 610         //SCANNER_DEBUG("%f %f %f", H, m_a*180/M_PI, offset_b);
 611
 612         //SCANNER_DEBUG("%d %2.2f ", a, tour_pos);
 613
 614
 615         //SCANNER_DEBUG("%f %f %ld %d", H, L, tour_pos, a);
 616
 617
 618         /*
 619         if (row_n%2){
 620                 //tour_pos = ((SCANNER_STEP_TOUR/2)-tour_pos);
 621                 tour_pos = (tour_pos*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
 622         }
 623         else{
 624                 tour_pos = ((SCANNER_STEP_TOUR-tour_pos)*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
 625         }
 626         */
 627         col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN);
 628         if (col_n>PIX_PER_SCAN)
 629                 printf("BUG!!! RECALC MAX L");
 630
 631         //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN;
 632
 633         //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS;
 634         //pos= row_n*PIX_PER_SCAN+tour_pos;
 635         //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos;
 636
 637
 638
 639         pos= row_n*PIX_PER_SCAN+col_n;
 640         last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n;
 641
 642         //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin);
 643
 644         //a-=0x100;
 645         a-=200;
 646         //a/=10;
 647
 648         if (0<= pos  && pos <MAX_SAMPLE)// && row_n%2)
 649                 //sample_tab[pos] =  a>0xff?0xFF:a;
 650                 //sample_tab[(int)L] = H ;
 651                 scan_params.sample_tab[pos] = H_fin;
 652         nop();
 653         if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){
 654                 /* we have a hole, pad it with minimal hight */
 655                 if (H_fin>scan_params.last_sample)
 656                         min_sample = scan_params.last_sample;
 657                 else
 658                         min_sample = H_fin;
 659
 660                 //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n);
 661
 662                 /* fill grow, avoid erasing curent pos */
 663                 if (pos > last_pos){
 664                         pos_tmp = last_pos;
 665                         last_pos = pos;
 666                         //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp);
 667
 668                 }
 669                 else{
 670                         pos_tmp = pos+1;
 671                         //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp);
 672                 }
 673
 674
 675                 for (;pos_tmp< last_pos;pos_tmp++){
 676                         if (0< pos_tmp && pos_tmp <MAX_SAMPLE)// && row_n%2)
 677                                 //sample_tab[pos_tmp] =  min_sample;
 678                         nop();
 679
 680                 }
 681
 682
 683         }
 684
 685         scan_params.last_row_n = row_n;
 686         scan_params.last_col_n = col_n;
 687         scan_params.last_sample = H_fin;
 688
 689
 690         //printf("pos : %ld", pos);
 691         //sample_tab[sample_i] =  a>0x1ff?0x1FF:a;
 692
 693         //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
 694
 695         /*
 696         if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
 697                 sample_tab[pos] = 0xffff;
 698         */
 699         scan_params.sample_i--;
 700 }
 701 #endif
 702
 703
 704
 705 void do_scan(void * dummy)
 706 {
 707
 708         int i, j;
 709         int16_t a;
 710         int32_t row_n;
 711         int32_t col_n;
 712
 713
 714         int32_t tour_pos;
 715         int32_t pos, last_pos;
 716         int32_t pos_tmp ;
 717         int32_t mot_pos;
 718         uint8_t min_sample;
 719         double H, L;
 720         int32_t H_fin, L_fin;
 721
 722         uint8_t flags;
 723
 724         union{
 725                 uint16_t u16;
 726                 lookup_h_l h_l;
 727         }val;
 728
 729
 730
 731         if (scan_params.sample_i==0)
 732                 return;
 733
 734
 735
 736         scan_params.max_column_detected = !!sensor_get(SCANNER_MAXCOLUMN_SENSOR);
 737
 738         mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
 739         if (scan_params.sample_i==1 || scan_params.max_column_detected || scan_params.must_stop  ){
 740
 741                 /* reset sample num in case of max column detected */
 742                 IRQ_LOCK(flags);
 743                 scan_params.sample_i = 1;
 744                 IRQ_UNLOCK(flags);
 745
 746
 747                 SCANNER_DEBUG("dump end enc %"PRIi32" %d ", mot_pos, PIX_PER_SCAN);
 748                 //scanner.flags &= (~CS_ON);
 749
 750                 /* stop scan at cur pos + 10 round */
 751                 mot_pos = encoders_spi_get_value_scanner_interpolation((void *)SCANNER_ENC);
 752                 mot_pos = SCANNER_STEP_TOUR * ((mot_pos/SCANNER_STEP_TOUR) + 1) ;
 753
 754                 SCANNER_DEBUG("set to %"PRIi32" ", mot_pos);
 755
 756                 cs_set_consign(&sensorboard.scanner.cs, mot_pos);
 757                 //pwm_ng_set(SCANNER_MOT_PWM, 0);
 758
 759
 760                 /* end working */
 761
 762                 //scanner_end_process();
 763                 if (!scan_params.must_stop  && !scan_params.max_column_detected)
 764                         process_img_to_zone(scan_params.sample_tab, PIX_PER_SCAN, MAX_SAMPLE/PIX_PER_SCAN);
 765
 766
 767                 scan_params.working = 0;
 768
 769         }
 770
 771         mot_pos-= scan_params.pos_start_scan;
 772
 773         a = adc_get_value( ADC_REF_AVCC | MUX_ADC13 );
 774
 775
 776         tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
 777
 778
 779         if (scan_params.debug != 0)
 780                 SCANNER_DEBUG("%ld %d ", tour_pos, a);
 781
 782         /* lookup in precomputed array */
 783
 784
 785         j = (DIM_DIST * (telemetre_to_cm(a)-TELEMETRE_MIN_CM))/(TELEMETRE_MAX_CM - TELEMETRE_MIN_CM);
 786         i = (DIM_ANGLE * tour_pos)/STEP_PER_ROUND;
 787
 788
 789         if (j<0)
 790                 j=0;
 791         if (j>=DIM_DIST)
 792                 j = DIM_DIST-1;
 793
 794         if (i>=DIM_ANGLE)
 795                 i = DIM_ANGLE-1;
 796
 797
 798         val.u16 = pgm_read_word_near(&array_h_l[j][i]);
 799
 800
 801         //val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]);
 802         //val.u16 = pgm_read_word_near(&array_h_l[a][tp]);
 803         H = val.h_l.h;
 804         L = val.h_l.l;
 805         /*
 806         val.u16 = pgm_read_word_near(&array_h_l[(a-TELEMETRE_MIN)/DIST_STEP][mot_pos/ANGLE_STEP]);
 807
 808         H = val.h_l.h;
 809         L = val.h_l.l;
 810         */
 811         H_fin = H;
 812         L_fin = L;
 813
 814
 815         if (L<=0)
 816                 L = 0;
 817
 818         col_n = (PIX_PER_SCAN*L)/(SCAN_L_MAX-SCAN_L_MIN);
 819         if (col_n>=PIX_PER_SCAN) {
 820                 //printf("BUG!!! RECALC MAX L");
 821                 col_n = PIX_PER_SCAN-1;
 822         }
 823
 824         //col_n = (PIX_PER_SCAN+col_n -5)%PIX_PER_SCAN;
 825
 826         //pos = (row_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS;
 827         //pos= row_n*PIX_PER_SCAN+tour_pos;
 828         //last_pos= last_row_n*PIX_PER_SCAN+last_tour_pos;
 829
 830         row_n = (mot_pos)/(SCANNER_STEP_TOUR/2);
 831
 832
 833         pos= row_n*PIX_PER_SCAN+col_n;
 834         last_pos= scan_params.last_row_n*PIX_PER_SCAN+scan_params.last_col_n;
 835
 836         //SCANNER_DEBUG("%ld %ld %ld %ld", row_n, col_n, pos, H_fin);
 837
 838         //a-=0x100;
 839         a-=200;
 840         //a/=10;
 841
 842         if (0<= pos  && pos <MAX_SAMPLE)// && row_n%2)
 843                 //sample_tab[pos] =  a>0xff?0xFF:a;
 844                 //sample_tab[(int)L] = H ;
 845                 scan_params.sample_tab[pos] = H_fin;
 846         nop();
 847         if ((scan_params.last_row_n == row_n) && ABS(last_pos-pos)>1){
 848                 /* we have a hole, pad it with minimal hight */
 849                 if (H_fin>scan_params.last_sample)
 850                         min_sample = scan_params.last_sample;
 851                 else
 852                         min_sample = H_fin;
 853
 854                 //printf("(%ld, %ld) (%ld %ld)", last_col_n, last_row_n, col_n, row_n);
 855
 856                 /* fill grow, avoid erasing curent pos */
 857                 if (pos > last_pos){
 858                         pos_tmp = last_pos;
 859                         last_pos = pos;
 860                         //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp);
 861
 862                 }
 863                 else{
 864                         pos_tmp = pos+1;
 865                         //printf("loop1 on (%ld, %ld) %ld", pos_tmp, last_pos, last_pos-pos_tmp);
 866                 }
 867
 868
 869                 for (;pos_tmp< last_pos;pos_tmp++){
 870                         if (0< pos_tmp && pos_tmp <MAX_SAMPLE){
 871                                 //scan_params.sample_tab[pos_tmp] =  min_sample;
 872                                 nop();
 873                         }
 874
 875                 }
 876
 877
 878         }
 879
 880         scan_params.last_row_n = row_n;
 881         scan_params.last_col_n = col_n;
 882         scan_params.last_sample = H_fin;
 883
 884
 885         //printf("pos : %ld", pos);
 886         //sample_tab[sample_i] =  a>0x1ff?0x1FF:a;
 887
 888         //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
 889
 890         /*
 891         if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
 892                 sample_tab[pos] = 0xffff;
 893         */
 894         IRQ_LOCK(flags);
 895         scan_params.sample_i--;
 896         IRQ_UNLOCK(flags);
 897
 898 }