예제 #1
0
/**
 * Checks if button_2 was pressed. Launch calibration or measurement action
 */
void measure_button()
{
	if (button_pressed_2)
	{
		if (calibrate)
		{
			printf("calibration necesary\n");
			calibration();
		}
		else
		{
			last_meassured_weight = measure_time_weight(1);
			printf("WEIGHT  %g \n", roundNo_f(last_meassured_weight, 2));
			delay_ms(2000);
		}
		button_pressed_2 = false;
	}
}
예제 #2
0
파일: main.c 프로젝트: andydev/spec_mtk
void perform(char *argv[])
{
	int num = 0;
	char cmd = argv[1][1];
	switch (cmd)
	{
	case 'c':
	case 'C':
		num = select_calibrate(argv[2][0]);		
		calibration(num);
		break;
	case 'n':
	case 'N':
		normal_mode();
		break;
		/*		case 'r':*/
		/*		case 'R':*/
		/*			raster_mode();*/
		/*			break;*/
/*	case 'd':*/
/*	case 'D':*/
/*		num = atoi(argv[2]);*/
/*		//debug_mode(num, DEBUG_MODE);*/
/*		break;*/

/*	case 'v':*/
/*	case 'V':*/
/*		get_version();*/
/*		break;*/
		/*	case 'i':*/
		/*	case 'I':*/
		/*	    num = atoi(argv[2]);*/
		/*		debug_mode(num, INTERNAL_MODE);*/
		/*		break;*/
/*	case 'b':*/
/*	case 'B':*/
/*		{*/
/*			char *fname = argv[2];*/
/*			bootloader_mode(fname);*/
/*		}*/
/*		break;*/
	}
}
예제 #3
0
void baro_MS5534A_init(void)
{
  /* 32768Hz on PWM2 */
  /* Configure P0.7 pin as PWM */
  PINSEL0 &= ~(_BV(14));
  PINSEL0 |= _BV(15);

  /* No prescaler */
  PWMPR = PWM_PRESCALER - 1;

  /* To get 32768Hz from a base frequency of 15MHz */
  PWMMR0 = PWM_PERIOD;
  PWMMR2 = PWM_DUTY;

  PWMLER = PWMLER_LATCH0 | PWMLER_LATCH2;
  PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
  PWMPCR = PWMPCR_ENA2;

#ifdef BARO_MS5534A_W1
  words[0] = BARO_MS5534A_W1;
  words[1] = BARO_MS5534A_W2;
  words[2] = BARO_MS5534A_W3;
  words[3] = BARO_MS5534A_W4;

  status = STATUS_MEASURE_PRESSURE;
  status_read_data = FALSE;

  calibration();
#else
  status = STATUS_INIT1;
  status_read_data = FALSE;
#endif



  baro_MS5534A_available = FALSE;
  alt_baro_enabled = FALSE;

  baro_MS5534A_ground_pressure = 101300;
  baro_MS5534A_r = 20.;
  baro_MS5534A_sigma2 = 1;
  baro_MS5534A_do_reset = FALSE;
}
예제 #4
0
int main(void)
 {
        //Init Motor
     /*
        BYTE_SET(DDRD,0xFE);
        PORTB = 0xFF;
        PORTB = 0xC0;
        DDRA  = 0x10;
        PORTA = 0x00;
       */ 
		
		initAdc();
		initUART();
		initTimer();
	//	DDRC=0xFF;
		

		//Enable interrupt after all init else Infinite loop
        sei();
        
       	calibration(); 

        //Enable_watchdog(WDTO_500MS);

	for (;;) 
	{  /* loop forever */
//		lireTelecommande();
/*		if(timerOvf == 1)
		{

		*/
	Vitesse_D = getUartVitesse();
	Angle_D = getUartAngle();
	Vg = getAvgSpeedMoteurGauche();
	Vd = getAvgSpeedMoteurDroit();

	CalculPWM(Vitesse_D,Angle_D,Vg,Vd,&Duty_G,&Duty_D);

		/*	timerOvf = 0;
		}
  */  
	}
}
void setup()
{
  Wire.begin(myAddress);
  Wire.onReceive(i2cReceive);
  Wire.onRequest(i2cRequest);

  pinMode(limit_switch_pin, INPUT); 

  mySerial.begin(9600); // Serial commuication to motor controller start.

  setpoint = 0.0;

  calibration(); // Running the calibration code on every start up

  input = encoderRead();

  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-127, 127);
  myPID.SetSampleTime(20);
}
void MainWindow::createConnections()
{
    connect(ui->actionNew, SIGNAL(triggered()), this, SLOT(newproject()));
    connect(ui->actionOpen, SIGNAL(triggered()), this, SLOT(openproject()));

    connect(ui->actionOpenCamera, SIGNAL(triggered()), this, SLOT(opencamera()));
    connect(ui->actionFocusAssistant, SIGNAL(triggered()), this, SLOT(startfocusassistant()));
    connect(ui->leftExSlider,SIGNAL(valueChanged(int)),this,SLOT(setexposure()));
    connect(ui->rightExSlider,SIGNAL(valueChanged(int)),this,SLOT(setexposure()));

    connect(ui->actionBasler, SIGNAL(triggered()), this, SLOT(usebasler()));//暂时用来调试的功能

    connect(ui->actionProjector,SIGNAL(triggered()),this,SLOT(projectorcontrol()));

    connect(ui->tabWidget, SIGNAL(currentChanged(int)), this, SLOT(selectPath(int)));

    connect(ui->actionCalib, SIGNAL(triggered()), this, SLOT(calib()));
    connect(ui->captureButton, SIGNAL(clicked()), this, SLOT(capturecalib()));
    connect(ui->redoButton, SIGNAL(clicked()), this, SLOT(redocapture()));
    connect(ui->calibButton,SIGNAL(clicked()),this,SLOT(calibration()));

    connect(ui->actionScan,SIGNAL(triggered()),this,SLOT(scan()));
    connect(ui->findPointButton,SIGNAL(clicked()),this,SLOT(pointmatch()));
    connect(ui->reFindButton,SIGNAL(clicked()),this,SLOT(refindmatch()));
    connect(ui->startScanButton, SIGNAL(clicked()), this, SLOT(startscan()));
    connect(ui->multiFreqTest, SIGNAL(clicked()), this, SLOT(testmulitfreq()));
    //connect(ui->testR, SIGNAL(clicked()), this, SLOT(test()));//用于测试功能的暂时按键

    connect(ui->actionReconstruct,SIGNAL(triggered()),this,SLOT(reconstruct()));
    connect(ui->reconstructionButton,SIGNAL(clicked()),this,SLOT(startreconstruct()));

    connect(ui->actionSet, SIGNAL(triggered()), this, SLOT(set()));
    connect(ui->actionChinese, SIGNAL(triggered()), this, SLOT(switchlanguage()));
    connect(ui->actionEnglish, SIGNAL(triggered()), this, SLOT(switchlanguage()));
    connect(ui->pSizeValue, SIGNAL(valueChanged(int)), this, SLOT(changePointSize(int)));
    connect(ui->loadTest, SIGNAL(clicked()), this, SLOT(loadTestModel()));

    connect(ui->actionExit, SIGNAL(triggered()), pj, SLOT(close()));//解决投影窗口不能关掉的问题
    connect(ui->actionExit, SIGNAL(triggered()), fa, SLOT(close()));
    connect(ui->actionExit, SIGNAL(triggered()), this, SLOT(close()));
}
예제 #7
0
int main(int argc, const char * argv[])
{
    // Change this calibration to yours:
    CameraCalibration calibration(545.31565719766058f, 545.31565719766058f, 326.0f, 183.5f);
    

    std::cout << "Input image not specified" << std::endl;
    std::cout << "Usage: markerless_ar_demo  [filepath to recorded video or image]" << std::endl;



    if (argc == 1)
    {
        processVideo(calibration, cv::VideoCapture());
    }
    else if (argc == 2)
    {
        std::string input = argv[1];
        cv::Mat testImage = cv::imread(input);
        if (!testImage.empty())
        {
            processSingleImage( calibration, testImage);
        }
        else 
        {
            cv::VideoCapture cap;
            if (cap.open(input))
            {
                processVideo( calibration, cap);
            }
        }
    }
    else
    {
        std::cerr << "Invalid number of arguments passed" << std::endl;
        return 1;
    }

    return 0;
}
예제 #8
0
void main(void)
{

	sei();
	INIT_IO();
	INIT_USART();
	INIT_TIMER();
	
	
	
	DDRB=0xff;
	DDRD=0x30;
	
	PORTB=0x06;
	PORTC=0xff;
	
	ADMUX=0x60;
	ADCSRA=0xcc;
	while(1)
	{
		
			
			while(calib_flg)
			{
				calibration();
			}
			
			
			sensing();
			Catch_position();
			OCR1A=position_R[position];
			OCR1B=position_L[position];
		
	}

}
예제 #9
0
void main(void) {

    /* Configure the oscillator for the device */
    ConfigureOscillator();



    /* Initialize I/O and Peripherals for application */
    InitApp();

    resetCS();

    startCSConversion();

    line1();
    writeLCD("START CALIBRATION");

    calibration();

    writeRegister(MODE, 0x00006000);
    writeRegister(VOLTAGE_GAIN, 0x2F9000);
    writeRegister(CURRENT_GAIN, 0x3Fe0000);

    startCSConversion();
    while (1) {
        uint24_t status = readRegister(STATUS_REG);
        line2();
        if (!(status & 0x1)) {
            convertExe(status);
            writeLCD(buffer);
            resetCS();
            startCSConversion();
            continue;
        }
        double readVal;
        const char * measure;
        switch (showStatus) {
            case ssConf:
                data.lData = readRegister(showStatus);
                sprintf(buffer, "%02X%02X%02X    ", data.bytes[2], data.bytes[1], data.bytes[0]);
                measure = "CONF    ";
                break;
            case ssCycleCount:
                data.lData = readRegister(showStatus);
                sprintf(buffer, "%d    ", data.lData);
                measure = "CYCLE    ";
                break;
            case ssStatus:
                data.lData = readRegister(showStatus);
                sprintf(buffer, "%02X%02X%02X    ", data.bytes[2], data.bytes[1], data.bytes[0]);
                measure = "STATUS    ";
                break;
            case ssMode:
                data.lData = readRegister(showStatus);
                sprintf(buffer, "%02X%02X%02X    ", data.bytes[2], data.bytes[1], data.bytes[0]);
                measure = "MODE    ";
                break;
            case ssTemperature:
                data.lData = readRegister(showStatus);
                sprintf(buffer, "%02X%02X%02X    ", data.bytes[2], data.bytes[1], data.bytes[0]);
                measure = "TEMP    ";
                break;
            case ssCurrentGain:
                data.lData = readRegister(showStatus);
                readVal = convertCurrentGain(data.lData);
                measure = "I GAIN  ";
                sprintf(buffer, "%f    ", readVal);
                break;
            case ssVoltageOffset:
                data.lData = readRegister(showStatus);
                readVal = convertOffset(data.lData);
                measure = "VDC offset";
                sprintf(buffer, "%f    ", readVal);
                break;
            case ssVoltageGain:
                data.lData = readRegister(showStatus);
                readVal = convertCurrentGain(data.lData);
                measure = "V GAIN  ";
                sprintf(buffer, "%f    ", readVal);
                break;
            case ssVoltageAcOffset:
                data.lData = readRegister(showStatus);
                readVal = convertOffset(data.lData);
                measure = "VAC offset";
                sprintf(buffer, "%f    ", readVal);
                break;

            case ssCurrent:
                data.lData = readRegister(showStatus);
                readVal = convertCurrent(data.lData);
                measure = "CURRENT ";
                sprintf(buffer, "%f    ", readVal);
                break;
            case ssVolt:
                data.lData = readRegister(INST_VOLT);
                readVal = convertInstVolt(data.lData);
                measure = "VOLT    ";
                sprintf(buffer, "%f    ", readVal);
                break;
            case ssRMSVoltage:
                data.lData = readRegister(ssRMSVoltage);
                readVal = convertRMSVolt(data.lData);
                sprintf(buffer, "%f    ", readVal);
                measure = "RMS VOLT";
                break;
            case ssRMSCurrent:
                data.lData = readRegister(ssRMSCurrent);
                readVal = convertRMSCurrent(data.lData);
                sprintf(buffer, "%f    ", readVal);
                measure = "RMS I";
                break;
            case ssPower:
                data.lData = readRegister(INST_POWER);
                readVal = convertPower(data.lData);
                measure = "I POWER  ";
                sprintf(buffer, "%f    ", readVal);
                break;
            case ssActivePower:
                data.lData = readRegister(ssActivePower);
                readVal = convertPower(data.lData);
                measure = "I POWER  ";
                sprintf(buffer, "%f    ", readVal);
                break;
            default:
                data.lData = readRegister(showStatus);
                sprintf(buffer, "%02X%02X%02X    ", data.bytes[2], data.bytes[1], data.bytes[0]);
                measure = "        ";
                break;
        }

        line1();
        writeLCD(buffer);
        line2();
        writeLCD(measure);
        if (PORTBbits.RB2 == 0) {
            if (PORTBbits.RB2 == 0) {
                showStatus++;
                if (showStatus == ssLast) {
                    showStatus = ssConf;
                }
                while (1) {
                    if (PORTBbits.RB2 == 1) {
                        if (PORTBbits.RB2 == 1) {
                            break;
                        }
                    }
                }
            }
        }
    }

}
예제 #10
0
int main(int argc, char *argv[])
{
	int c;
	int clone_err;
	int i,rc;
	int exit_rc = 0;
	int wait_status;
	int iterations = 0;
	struct sched_param param;
	struct rlimit myrlimit;
	struct sembuf mysembuf1;

	num_active = num_children ; /* In case its unspecified */

	while ((c = getopt(argc, argv, "c:t:a:w:r:o:q")) != -1) {
	  switch (c) {
	  case 'c': num_children = atoi(optarg); break;
	  case 't': num_seconds = atoi(optarg); break;
	  case 'a': num_active = atoi(optarg); break;
	  case 'w': weight_reschedule_idle = atoi(optarg); break;
	  case 'r': rnd_compute_time = atoi(optarg); break; 	  
	  case 'o': foutput = atoi(optarg); break;
	  case 'q': fquiet = 1; break;
	  default:  usage();
	    goto exit_main2;
	  }
	}


	
	
	if ((num_seconds <= 0) ||
	    (num_children <= 0) || (num_children > MAX_CHILDREN) ||
	    (num_active <= 0) || (num_active > num_children) ||
	    (weight_reschedule_idle < 0) || (weight_reschedule_idle > 100) ||
	    (rnd_compute_time < 0)
	   )
	    
	{
	    	usage();
		goto exit_main2;
	}
	

	/* Increase limits on number of open files */
	/* normally 1024 (cur & max), set to MAX_CHILDREN */

	myrlimit.rlim_cur = myrlimit.rlim_max = MAX_CHILDREN*2 ; 

	if (setrlimit(RLIMIT_NOFILE,&myrlimit) != 0) {
		exit_rc = errno ;
		perror("setrlimit() ");
		goto exit_main2;
	}
	
	/* allocate childrens stacks*/
	
	child_stack = malloc(num_children*STACK_SIZE);
	if (child_stack == NULL) {
		exit_rc = errno;
		perror ("malloc of 'child_stack' failed ");
		goto exit_main2;
	}
	
	/* open num_children pipes */
	for (i=0; i< num_children; i++) {
		if (pipe(childpipe[i]) < 0) {
			exit_rc = errno;
			perror ("pipe() ");
			goto exit_main3;
		}
	}

        /* calibrate internal loops */
	calibration();
	probyield = local_exec_count ;
	TOKENSZ = sizeof(char); 
	probyield = (100.0-(double)weight_reschedule_idle)/100.0 ; 
	probyieldmult = probyield / ((1-probyield)*(1-probyield)); 

	
	/*
	comp_nonyield_rounds = (int) (uniform(rnd_compute_time) * rounds_per_microsecond) ;
	comp_yield_rounds = (int) (comp_nonyield_rounds * probyieldmult) ; 
	printf("Sample subrounds : nonyield %d yield %d, probyield %0.3f\n",
	   comp_nonyield_rounds, comp_yield_rounds, probyield); 
	*/

	/* start_sem is used to start all children at same time */	

	start_sem = semget (IPC_PRIVATE, 1, IPC_CREAT | IPC_EXCL | 0660);
	if (start_sem == -1) {
		exit_rc = errno;
		perror("semget(start_sem) IPC_CREATE ");
		goto exit_main4;
	}
	
	/* allocate/initialize statistic variables */
	
	total = malloc(num_children*sizeof(struct _total));
	if (total == NULL) {
		exit_rc = errno;
		perror ("malloc of 'total' failed ");
		goto exit_main3;
	}
	for (i = 0 ; i < num_children ; i++)
		total[i].count = 0;


	/* Launch threads */
  	worker = bouncer;
	for (i=0; i< num_children; i++) {
		clone_err = __clone(worker, 
				    &child_stack[(i+1)*STACK_SIZE],
				    CLONE_FLAGS,
				    (void*)i);
		if (clone_err == -1) {
			exit_rc = errno;
			perror ("clone() ");
			goto exit_main5;
		}
		// printf("\t\tLaunched child %d\n",i);

	}

	/* Increase priority of parent thread */

	param.sched_priority = 90;
	rc = sched_setscheduler(getpid(), SCHED_FIFO, &param);
	if (rc == -1) {
		exit_rc = errno;
		perror ("sched_setscheduler() ");
		goto exit_main5;
	}

	run_test_time();
	

exit_main5: 
	/* wait until all children complete */	
	for (i = 0 ; i < num_children ; i++) {
		rc = waitpid (-1, &wait_status, __WCLONE);
		if (rc == -1) {
			exit_rc = errno;
			perror ("waitpid() ");
		}
	}

exit_main4:
 
	rc = semctl(start_sem, 0, IPC_RMID, 0);

exit_main3:

	/* explicitly close all pipes : unnecessary */
	for (i=0; i< num_children; i++) {
		close(childpipe[i][0]);
		close(childpipe[i][1]);
	}
	free(child_stack);
	
exit_main2:
	return (exit_rc) ;
}
예제 #11
0
파일: main.cpp 프로젝트: TBFMX/opencvWork
int main(int argc, const char * argv[])
{
	// franquy parameters
	float fx = 695.4521167717107;
	float fy = 694.5519610122569;
	float cx = 337.2059936807979;
	float cy = 231.1645822893514;
	
	// tablet parameters
	fx=628.6341119951087;
	fy=628.7519411113429;
	cx=325.3443919995285;
	cy=236.0028199018263;
	
    // Change this calibration to yours:
    //~ CameraCalibration calibration(526.58037684199849f, 524.65577209994706f, 318.41744018680112f, 202.96659047014398f);
    CameraCalibration calibration(fx,fy,cx,cy);
    
    if (argc < 2)
    {
        std::cout << "Input image not specified" << std::endl;
        std::cout << "Usage: markerless_ar_demo <pattern image> [filepath to recorded video or image]" << std::endl;
        return 1;
    }

    // Try to read the pattern:
    cv::Mat patternImage = cv::imread(argv[1]);
    if (patternImage.empty())
    {
        std::cout << "Input image cannot be read" << std::endl;
        return 2;
    }

    if (argc == 2)
    {
		cv::VideoCapture cap(0);
        processVideo(patternImage, calibration, cap);
    }
    else if (argc == 3)
    {
        std::string input = argv[2];
        cv::Mat testImage = cv::imread(input);
        if (!testImage.empty())
        {
            processSingleImage(patternImage, calibration, testImage);
        }
        else 
        {
            cv::VideoCapture cap(0);
            if (cap.open(input))
            {
                processVideo(patternImage, calibration, cap);
            }
        }
    }
    else
    {
        std::cerr << "Invalid number of arguments passed" << std::endl;
        return 1;
    }

    return 0;
}
예제 #12
0
파일: main.c 프로젝트: andydev/spec_mtk
void execute(void)
{
	int num, type = 1;
	char cmd, flag = 'n';
	char fname[64] = "/auto.pix";

	main_menu();
	while (1)
	{
 	    printf("Input cmd: ");
		cmd = getchar();
		while(getchar()!='\n');	
		switch (cmd)
		{
		case 't':
		case 'T':			
			ioctl(gldev, RESET_TP, 0);
			break;

		case 'c':
		case 'C':
			//num = input_calibrate_menu();					    
			//num = select_calibrate(cmd);	
			num = 1;		
			calibration(num);
			break;

/*		case 'n':*/
/*		case 'N':*/
/*			normal_mode();*/
/*			break;*/
			/*		case 'r':*/
			/*		case 'R':*/
			/*			raster_mode();*/
			/*			break;*/

/*		case 'd':*/
/*		case 'D':*/
/* 			input_debug_param(&type,&flag);*/
/*			debug_mode(type, flag);*/
/*			break;*/

		case 'b':
		case 'B':
			input_fname(fname);			
			bootloader_mode(fname);
			break;

/*		case 'v':*/
/*		case 'V':*/
/*			get_version();*/
/*			break;*/
/*		*/
/*		case 'i':*/
/*		case 'I':*/
/*		    input_debug_param(&type,&flag);*/
/*			internal_mode(type, flag);*/
/*		    break;*/

			/*		case 't':	*/
			/*		case 'T':*/
			/*		    button_raw();*/
			/*		    break;	*/

/*		case 'r':*/
/*		case 'R':*/
/*			read_eeprom();*/
/*			break;*/

/*		case 'w':*/
/*		case 'W':*/
/*			write_eeprom();*/
/*			break;*/

		case 'm':
		case 'M':
			main_menu();
			break;

		case 'x':
		case 'X':
			return;
		}			
	}
}
예제 #13
0
파일: main.c 프로젝트: andydev/spec_mtk
int bootloader_mode(char *fname)
{
	int i, ret, len, arg = 0;
	FILE *fp;
	char wbuf[WBUF];	
	unsigned char flag = 1;
	unsigned char reset = 0;

	fp = fopen(fname, "r");
	if (fp == NULL)
	{
		puts("Open file error");
		return -1;
	}
	ioctl(gldev, DISABLE_IRQ, 0);
	ioctl(gldev, BOOTLOADER_MODE, arg);	
	while (1)
	{
		char rbuf[RBUF];
		fgets(rbuf, RBUF, fp);
		if (feof(fp)) break;		
		if( !strncmp(rbuf, "!delay", 6)) 
		{	
			printf("%s", rbuf);		
			msleep(300);
			continue;
		}
		else if ( rbuf[0]!='$') continue;
		else if( !strncmp(rbuf, "$00", 3))
		{
			flag = 0;
		}
		if( !strncmp(rbuf, "$03", 3) && flag )
		{
			reset = 1;
			flag = 0;
			for (i = 0; i < WBUF; i++)
			{
				wbuf[i] = 0;
			}
			
			ret = write(gldev, wbuf, WBUF);
			msleep(150);
			if (ret != WBUF)
			{
				ioctl(gldev, ENABLE_IRQ, 0);
				fclose(fp);
				puts("Download failed.");
				return 1;
			}
		}
		
		for (i = 0; i < WBUF; i++)
		{
			wbuf[i] = (hex2char(rbuf[(i + 1) * 2 - 1]) << 4) | hex2char(rbuf[(i + 1) * 2]);
		}
		printf("%s", rbuf);		
		ret = write(gldev, wbuf, WBUF);
		msleep(150); 
		if (ret != WBUF)
		{
			ioctl(gldev, ENABLE_IRQ, 0);
			fclose(fp);
			puts("Download failed.");
			return 1;
		}
	}

	ioctl(gldev, ENABLE_IRQ, 0);
	fclose(fp);
	puts("Download successed.");

	if(ret==WBUF && reset )
	{
		ioctl(gldev, RESET_TP, 0);
		sleep(1);
		calibration(1);
	}

	return 1;
}
int CompassSensor::readEvents(sensors_event_t* data, int count)
{
    if (count < 1)
        return -EINVAL;

    ssize_t n = mInputReader.fill(data_fd);
    if (n < 0)
        return n;

    int numEventReceived = 0;
    input_event const* event;

    D("%s count = %d ", __func__, count);

    while (count && mInputReader.readEvent(&event)) {
        D("readEvents event->type = %d, code=%d, value=%d",
          event->type, event->code, event->value);

        int type = event->type;
        if (type == EV_REL && !inputDataOverrun) {
            if (event->code == EVENT_TYPE_M_O_X)
                mMagneticEvent.data[mConfig->mapper[AXIS_X]] =
                        COMPASS_CONVERT(event->value, mConfig->scale[AXIS_X]);
            else if (event->code == EVENT_TYPE_M_O_Y)
                mMagneticEvent.data[mConfig->mapper[AXIS_Y]] =
                        COMPASS_CONVERT(event->value, mConfig->scale[AXIS_Y]);
            else if (event->code == EVENT_TYPE_M_O_Z)
                mMagneticEvent.data[mConfig->mapper[AXIS_Z]] =
                        COMPASS_CONVERT(event->value, mConfig->scale[AXIS_Z]);
        } else if (type == EV_SYN) {
            int64_t time = timevalToNano(event->time);

            /* drop input event overrun data */
            if (event->code == SYN_DROPPED) {
                E("CompassSensor: input event overrun, dropped event:drop");
                inputDataOverrun = 1;
            } else if (inputDataOverrun) {
                E("CompassSensor: input event overrun, dropped event:sync");
                inputDataOverrun = 0;
            } else {
                if (mEnabled) {
                    mMagneticEvent.timestamp = time;

                    /* compass calibration */
                    calibration(time);

                    D("CompassSensor magnetic befor filter=[%f, %f, %f] accuracy=%d, time=%lld",
                        mMagneticEvent.magnetic.x,
                        mMagneticEvent.magnetic.y,
                        mMagneticEvent.magnetic.z,
                        (int)mMagneticEvent.magnetic.status,
                        mMagneticEvent.timestamp);

                    /* data filter: used to mitigate data floating */
                    if (mFilterEn)
                        filter();

                    *data++ = mMagneticEvent;
                    count--;
                    numEventReceived++;
                    D("CompassSensor magnetic=[%f, %f, %f] accuracy=%d, time=%lld",
                      mMagneticEvent.magnetic.x,
                      mMagneticEvent.magnetic.y,
                      mMagneticEvent.magnetic.z,
                      (int)mMagneticEvent.magnetic.status,
                      mMagneticEvent.timestamp);
                }
            }
        } else {
            E("CompassSensor: unknown event (type=%d, code=%d)",
              type, event->code);
        }
        mInputReader.next();
    }

    return numEventReceived;
}
int
main(int argc, char** argv)
{
    cv::Size boardSize;
    float squareSize;
    double delay;
    int imageCount;
    std::string cameraModel;
    std::string cameraNs;
    float minMove;

    //========= Handling Program options =========
    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("width,w", boost::program_options::value<int>(&boardSize.width)->default_value(8), "Number of inner corners on the chessboard pattern in x direction")
        ("height,h", boost::program_options::value<int>(&boardSize.height)->default_value(5), "Number of inner corners on the chessboard pattern in y direction")
        ("size,s", boost::program_options::value<float>(&squareSize)->default_value(120.f), "Size of one square in mm")
        ("delay", boost::program_options::value<double>(&delay)->default_value(0.5), "Minimum delay in seconds between captured images")
        ("count", boost::program_options::value<int>(&imageCount)->default_value(50), "Number of images to be taken for the calibration")
        ("camera-model", boost::program_options::value<std::string>(&cameraModel)->default_value("mei"), "Camera model: kannala-brandt | mei | pinhole")
        ("camera-ns", boost::program_options::value<std::string>(&cameraNs)->default_value("cam"), "Camera namespace")
        ("move,m", boost::program_options::value<float>(&minMove)->default_value(20.f), "Minimal move in px of chessboard to be used")
        ;

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
    boost::program_options::notify(vm);

    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 1;
    }

    px::Camera::ModelType modelType;
    if (boost::iequals(cameraModel, "kannala-brandt"))
    {
        modelType = px::Camera::KANNALA_BRANDT;
    }
    else if (boost::iequals(cameraModel, "mei"))
    {
        modelType = px::Camera::MEI;
    }
    else if (boost::iequals(cameraModel, "pinhole"))
    {
        modelType = px::Camera::PINHOLE;
    }
    else
    {
        ROS_ERROR("Unknown camera model: %s", cameraModel.c_str());
        return 1;
    }

    switch (modelType)
    {
    case px::Camera::KANNALA_BRANDT:
        ROS_INFO("Camera model: Kannala-Brandt");
        break;
    case px::Camera::MEI:
        ROS_INFO("Camera model: Mei");
        break;
    case px::Camera::PINHOLE:
        ROS_INFO("Camera model: Pinhole");
        break;
    }

    ros::init(argc, argv, "mono_camera_calibration");

    ros::NodeHandle nh;

    ros::Subscriber cameraInfoSub;
    px_comm::CameraInfoPtr cameraInfo;
    cameraInfoSub = nh.subscribe<px_comm::CameraInfo>(ros::names::append(cameraNs, "camera_info"), 1,
                                                      boost::bind(cameraInfoCallback, _1, boost::ref(cameraInfo)));

    ROS_INFO("Waiting for camera information...");

    ros::Rate r(50);
    while (ros::ok() && cameraInfo.get() == 0)
    {
        ros::spinOnce();
        r.sleep();
    }

    cameraInfoSub.shutdown();

    if (cameraInfo.get() == 0)
    {
        ROS_ERROR("Aborted calibration due to missing camera information.");
        return 1;
    }

    ROS_INFO("Received camera information.");
    ROS_INFO("   Camera name: %s", cameraInfo->camera_name.c_str());
    ROS_INFO("   Image width: %d", cameraInfo->image_width);
    ROS_INFO("  Image height: %d", cameraInfo->image_height);

    px::CameraCalibration calibration(modelType, cameraInfo->camera_name,
                                      cv::Size(cameraInfo->image_width,
                                               cameraInfo->image_height),
                                      boardSize, squareSize / 1000.0f);
    calibration.setVerbose(true);

    cv::namedWindow("Image");

    image_transport::ImageTransport it(nh);
    px::AtomicContainer<cv::Mat> frame;
    image_transport::Subscriber imageSub;
    imageSub = it.subscribe(ros::names::append(cameraNs, "image_raw"), 1,
                            boost::bind(imageCallback, _1, boost::ref(frame)));

    cv::Point2f lastFirstCorner = cv::Point2f(std::numeric_limits<float>::max(),
                                              std::numeric_limits<float>::max());
    ros::Time lastFrameTime;

    cv::Mat imgView;
    while (ros::ok() && calibration.sampleCount() < imageCount)
    {
        ros::spinOnce();
        r.sleep();

        if (!frame.available())
        {
            continue;
        }

        px::Chessboard chessboard(boardSize, frame.data());
        chessboard.findCorners();

        if (chessboard.cornersFound())
        {
            chessboard.getSketch().copyTo(imgView);
        }
        else
        {
            frame.data().copyTo(imgView);
        }

        if (chessboard.cornersFound() &&
            (frame.timestamp() - lastFrameTime).toSec() > delay &&
            cv::norm(cv::Mat(lastFirstCorner - chessboard.getCorners()[0])) > minMove)
        {
            lastFirstCorner = chessboard.getCorners()[0];
            lastFrameTime = frame.timestamp();

            cv::bitwise_not(imgView, imgView);
            calibration.addChessboardData(chessboard.getCorners());
        }

        std::ostringstream oss;
        oss << calibration.sampleCount() << " / " << imageCount;

        cv::putText(imgView, oss.str(), cv::Point(10,20),
                    cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),
                    1, CV_AA);

        cv::imshow("Image", imgView);
        cv::waitKey(2);

        frame.available() = false;
    }

    cv::destroyWindow("Image");

    if (calibration.sampleCount() < imageCount)
    {
        ROS_ERROR("Aborted calibration due to insufficient number of detected chessboards.");
        return 1;
    }

    ROS_INFO("Calibrating...");

    ros::Time startTime = ros::Time::now();

    calibration.calibrate();
    calibration.writeParameters(cameraInfo->camera_name + "_camera_calib.yaml");
    calibration.writeParameters(cameraInfo);
    calibration.writeChessboardData(cameraInfo->camera_name + "_chessboard_data.dat");

    ROS_INFO("Calibration took a total time of %.1f sec.",
             (ros::Time::now() - startTime).toSec());

    ROS_INFO("Wrote calibration file to %s",
             (cameraInfo->camera_name + "_camera_calib.yaml").c_str());

    // send SetCameraInfo request
    ros::ServiceClient cameraInfoClient;
    cameraInfoClient = nh.serviceClient<px_comm::SetCameraInfo>(ros::names::append(cameraNs, "set_camera_info"));

    px_comm::SetCameraInfo setCameraInfo;
    setCameraInfo.request.camera_info = *cameraInfo;

    if (cameraInfoClient.call(setCameraInfo))
    {
        ROS_INFO("Received reply to SetCameraInfo request.");

        if (setCameraInfo.response.success == true)
        {
            ROS_INFO("SetCameraInfo request was processed successfully: %s.",
                     setCameraInfo.response.status_message.c_str());
        }
        else
        {
            ROS_ERROR("SetCameraInfo request was processed successfully.");
        }
    }
    else
    {
        ROS_ERROR("Did not receive reply to SetCameraInfo request.");
    }

    cv::Mat mapX, mapY;
    px::CameraPtr& camera = calibration.camera();

    if (camera->modelType() == px::Camera::PINHOLE)
    {
        camera->initUndistortRectifyMap(mapX, mapY);
    }
    else
    {
        camera->initUndistortRectifyMap(mapX, mapY, 300.0f, 300.0f,
                                        cv::Size(camera->imageWidth(),
                                                 camera->imageHeight()),
                                        -1.0f, -1.0f);
    }

    while (ros::ok())
    {
        ros::spinOnce();
        r.sleep();

        if (!frame.available())
        {
            continue;
        }

        cv::remap(frame.data(), imgView, mapX, mapY, cv::INTER_LINEAR);

        cv::putText(imgView, "Calibrated", cv::Point(10,20),
                    cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255),
                    1, CV_AA);

        cv::imshow("Image", imgView);
        cv::waitKey(2);

        frame.available() = false;
    }

    return 0;
}
예제 #16
0
파일: main.c 프로젝트: xidiandaily/drawtree
int main(int argc,char* argv[])
{
    T_NODE* head=NULL;
    DRAW_TYPE type;
    int prev_begin=0;
    int prev_end=0;
    int mid_begin=0;
    int mid_end=0;

#ifndef __DEBUG__
    if(argc != 4)
    {
        help();
        return 0;
    }
    else
    {
        if(!strcmp(argv[1],"-null"))
        {
            type = SHOW_NULL;
        }
        else if(!strcmp(argv[1],"-nnull"))
        {
            type = UNSHOW_NULL;
        }
        else
        {
            help();
            return 0;
        }
        pPrev=(char*)malloc(sizeof(strlen(argv[2])+1));
        memset(pPrev,0,(sizeof(strlen(argv[2])+1)));
        strcpy(pPrev,argv[2]);
        prev_begin=0;
        prev_end=strlen(argv[2])-1;


        pMid=(char*)malloc(sizeof(strlen(argv[3])+1));
        memset(pMid,0,(sizeof(strlen(argv[3])+1)));
        strcpy(pMid,argv[3]);
        mid_begin=0;
        mid_end=strlen(argv[3])-1;
    }

#else
        pPrev=(char*)malloc(sizeof(strlen(prev)+1));
        memset(pPrev,0,(sizeof(strlen(prev)+1)));
        strcpy(pPrev,prev);
        prev_begin=0;
        prev_end=strlen(prev)-1;


        pMid=(char*)malloc(sizeof(strlen(mid)+1));
        memset(pMid,0,(sizeof(strlen(mid)+1)));
        strcpy(pMid,mid);
        mid_begin=0;
        mid_end=strlen(mid)-1;
#endif
    buildtree(&head,prev_begin,prev_end,mid_begin,mid_end);
    if(!calibration(head,pPrev,pMid))
    {
        printf("FAIL!!!  can't build binary tree, please check \n");
    }
    else
    {
        drawtree_build_googleAPI_html(head,type,"binarytree_graphic.html");
    }

    return 0;
}
/**
 * \brief Main command interpreter function
 *
 * This operator analyzes the command arguments and calls the appropriate
 * subcommand method.
 */
void	guidercommand::operator()(const std::string& /* command */,
		const std::vector<std::string>& arguments) {
	if (arguments.size() < 2) {
		throw command_error("guider command requires more "
			"arguments");
	}
	std::string	guiderid = arguments[0];
	std::string	subcommand = arguments[1];
	debug(LOG_DEBUG, DEBUG_LOG, 0, "guiderid: %s", guiderid.c_str());

	Guiders	guiders;
	GuiderWrapper	guider = guiders.byname(guiderid);

	if (subcommand == "info") {
		info(guider, arguments);
		return;
	}

	if (subcommand == "exposure") {
		exposure(guider, arguments);
		return;
	}

	if (subcommand == "exposuretime") {
		exposuretime(guider, arguments);
		return;
	}

	if (subcommand == "binning") {
		binning(guider, arguments);
		return;
	}

	if (subcommand == "size") {
		size(guider, arguments);
		return;
	}

	if (subcommand == "offset") {
		offset(guider, arguments);
		return;
	}

	if (subcommand == "star") {
		star(guider, arguments);
		return;
	}

	if (subcommand == "calibration") {
		calibration(guider, arguments);
		return;
	}

	if (subcommand == "start") {
		start(guider, arguments);
		return;
	}

	if (subcommand == "stop") {
		stop(guider, arguments);
		return;
	}

	if (subcommand == "wait") {
		wait(guider, arguments);
		return;
	}

	if (subcommand == "image") {
		image(guider, arguments);
		return;
	}
}
예제 #18
0
/**
 * tries to load the camera calibration
 *
 * The camera calibration with the format of the libvisensor < 2.0.0 is loaded and parsed to
 * ViCameraCalibration vector. If no Xml format could be found or parsed a empty vector is returned.
 *
 * @return vector of the saved configurations
 *
 */
std::vector<visensor::ViCameraCalibration> SensorUpdater::parseXmlCameraCalibration(
    const std::string& xml_filename)
{
  std::vector<visensor::ViCameraCalibration> output_vector;
  boost::property_tree::ptree calibration_tree;

  if (!loadPropertyTree(xml_filename, &calibration_tree)) {
    exit(1);
  }

  BOOST_FOREACH(const boost::property_tree::ptree::value_type & iter, calibration_tree.get_child("") ){
    visensor::ViCameraCalibration calibration(visensor::ViCameraLensModel::LensModelTypes::RADIAL,
        visensor::ViCameraProjectionModel::ProjectionModelTypes::PINHOLE);
    std::vector<std::string> elements;
    boost::split(elements, iter.first, boost::is_any_of("_"));

    if( (elements.size() == 3) && (elements[0] == "cam") ) {
      try
      {
        calibration.cam_id_ = std::stoi(elements[1]);
        calibration.slot_id_ = std::stoi(elements[2])/2;
        calibration.is_flipped_ = std::stoi(elements[2])%2;
        calibration.resolution_[0] = 752;
        calibration.resolution_[1] = 480;
        //build childtree name
        std::string cam_id_str = boost::lexical_cast<std::string>(calibration.cam_id_);
        std::string slot_str = boost::lexical_cast<std::string>(std::stoi(elements[2]));
        std::string child_tree = std::string("cam_") + cam_id_str + std::string("_") + slot_str + std::string(".");

        //load data
        visensor::ViCameraLensModelRadial::Ptr lens_model = calibration.getLensModel<visensor::ViCameraLensModelRadial>();
        visensor::ViCameraProjectionModelPinhole::Ptr projection_model = calibration.getProjectionModel<visensor::ViCameraProjectionModelPinhole>();
        projection_model->focal_length_u_ = calibration_tree.get<double>(child_tree + "fu");
        projection_model->focal_length_v_ = calibration_tree.get<double>(child_tree + "fv");
        projection_model->principal_point_u_ = calibration_tree.get<double>(child_tree + "cu");
        projection_model->principal_point_v_ = calibration_tree.get<double>(child_tree + "cv");

        lens_model->k1_ = calibration_tree.get<double>(child_tree + "K0");
        lens_model->k2_ = calibration_tree.get<double>(child_tree + "K1");
        lens_model->r1_ = calibration_tree.get<double>(child_tree + "K2");
        lens_model->r2_ = calibration_tree.get<double>(child_tree + "K3");

        calibration.R_.resize(9);
        calibration.R_[0] = calibration_tree.get<double>(child_tree + "R00");
        calibration.R_[1] = calibration_tree.get<double>(child_tree + "R01");
        calibration.R_[2] = calibration_tree.get<double>(child_tree + "R02");
        calibration.R_[3] = calibration_tree.get<double>(child_tree + "R10");
        calibration.R_[4] = calibration_tree.get<double>(child_tree + "R11");
        calibration.R_[5] = calibration_tree.get<double>(child_tree + "R12");
        calibration.R_[6] = calibration_tree.get<double>(child_tree + "R20");
        calibration.R_[7] = calibration_tree.get<double>(child_tree + "R21");
        calibration.R_[8] = calibration_tree.get<double>(child_tree + "R22");

        calibration.t_.resize(3);
        calibration.t_[0] = calibration_tree.get<double>(child_tree + "t0");
        calibration.t_[1] = calibration_tree.get<double>(child_tree + "t1");
        calibration.t_[2] = calibration_tree.get<double>(child_tree + "t2");

      } catch(std::exception const& ex)
      {
        std::cout << "failed.\n";
        std::cout << "Exception: " << ex.what() << "\n";
        return output_vector;
      }
      output_vector.push_back(calibration);
    }
  }
  return output_vector;
}
예제 #19
0
int LDWS(unsigned char *img, int width, int height, PointKAIST *pt_left, PointKAIST *pt_right)
{

	int result = 0, result2 = 0, i = 0;

	///////////calibration(img, width, height, pt_left, pt_right);

	search_lane(img, width, height-bottom_margin, pt_left, pt_right);

	ransac(img, pt_left, pt_right, 10);

	lane_filtering(pt_left, pt_right, width, height);

	// 일정 프레임(현재 100)까지 calibration을 수행하도록 하는 구문
	if(calibration_frame < 100)
	{
		calibration(pt_left, pt_right, width, height);
		calibration_frame++;
	}

	//result = warning_system2(pt_left, pt_right);
	//tracking(pt_left, pt_right, width, height);


	if(pt_left[0].x == 0 && pt_left[0].y == 0)
		init(pt_left, pt_right);

	if(pt_right[0].x == 0 && pt_right[0].y == 0)
		init(pt_left, pt_right);

/**
	// 기준선 그리기
	for(i=0; i<width; i++)
	{
		img[horizontal_center*width+i-1] = 255;
		img[horizontal_center*width+i] = 255;
		img[horizontal_center*width+i+1] = 255;
	}

	for(i=0; i<height; i++)
	{
		img[i*width+vertical_center-1] = 255;
		img[i*width+vertical_center] = 255;
		img[i*width+vertical_center+1] = 255;
	}
**/
	//printf("noise : %f\n", get_noise(img, width, height, pt_left[1].x, pt_right[1].x, pt_left[1].y));
/*
	//printf("%d ", pt_left[0].x);
	if((pt_right[0].x - pt_left[0].x) > 300 && pt_right[0].x != 0 && pt_right[0].y != 0 && pt_left[0].x != 0 && pt_right[0].y != 0)
		// 한쪽만 움직인 경우는 제외함
	{
		if((prev_prev_pt_left[0].x - pt_left[0].x) != 0 && (prev_prev_pt_right[0].x - pt_right[0].x) != 0)
		//printf("%d %d\n", pt_left[0].x, pt_right[0].x);


		// vertical_center로 교체됨
			result = warning_system(vertical_center, (pt_right[0].x - pt_left[0].x) / 2 + pt_left[0].x, warning_val, pt_left[0].x, pt_right[0].x);
	}
	else
	{
		result = 0;
		init(pt_left, pt_right);
	}
*/
	result = warning_system(vertical_center, (pt_right[0].x - pt_left[0].x) / 2 + pt_left[0].x, warning_val, pt_left[0].x, pt_right[0].x);
	//draw_lane(img, width, height, pt_left[0].x, pt_left[0].y, pt_right[0].x, pt_right[0].y, pt_left[2].x, pt_left[2].y, pt_right[2].x, pt_right[2].y, pt_left, pt_right);

	// 오른쪽
	if(result==1)
	{
		// 튀는 경우 방지
		if(start == 0 && prev_warning > 0)
		{
			//draw_warning(img, width, height);
			result2 = result;

			init(pt_left, pt_right);
		}
		else
			result2 = 0;
	}

	// 왼쪽
	else if(result==2)
	{
		if(start == 0 && prev_warning > 0)
		{
			//draw_warning2(img, width, height);
			result2 = result;

			init(pt_left, pt_right);
		}
		else
			result2 = 0;
	}
	else
	{
		start = 0;
		result = 0;
		result2 = 0;
	}

	prev_prev_pt_left[0].x = pt_left[0].x;
	prev_prev_pt_left[0].y = pt_left[0].y;
	prev_prev_pt_left[1].x = pt_left[1].x;
	prev_prev_pt_left[1].y = pt_left[1].y;
	prev_prev_pt_left[2].x = pt_left[2].x;
	prev_prev_pt_left[2].y = pt_left[2].y;

	prev_prev_pt_right[0].x = pt_right[0].x;
	prev_prev_pt_right[0].y = pt_right[0].y;
	prev_prev_pt_right[1].x = pt_right[1].x;
	prev_prev_pt_right[1].y = pt_right[1].y;
	prev_prev_pt_right[2].x = pt_right[2].x;
	prev_prev_pt_right[2].y = pt_right[2].y;

//	draw_test(img, width, height);


	//printf("warning : %d %d\n", prev_warning, result);

	//오른쪽 차선 1
	if(prev_warning == 1 && result == 2)
	{
		//draw_warning(img, width, height);
		//init(pt_left, pt_right);
		return 1;
	}
	// 왼쪽 차선 2
	else if(prev_warning == 2 && result == 1)
	{
		//draw_warning2(img, width, height);
		//init(pt_left, pt_right);
		return 2;
	}

	prev_warning = result;

	return 0;
}
예제 #20
0
int main( void )
{
	// Enable interrupts as early as possible
	sei();
	
	Timer_Init();

	// Set as outputs and stop rotor
	DDRD |= (1 << PD1)|(1 << PD2);
	PORTD &= ~((1 << PD1)|(1 << PD2));
	
	// Setup ADC
	initAdcFeedback();
	
	Can_Message_t txMsg;
	txMsg.Id = (CAN_NMT_APP_START << CAN_SHIFT_NMT_TYPE) | (NODE_ID << CAN_SHIFT_NMT_SID);
	txMsg.DataLength = 4;
	txMsg.RemoteFlag = 0;
	txMsg.ExtendedFlag = 1;
	txMsg.Data.words[0] = APP_TYPE;
	txMsg.Data.words[1] = APP_VERSION;
	
	// Set up callback for CAN reception
	BIOS_CanCallback = &can_receive;
	// Send CAN_NMT_APP_START
	BIOS_CanSend(&txMsg);
	
	// Read calibration value from eeprom
	azimuthCalibration = eeprom_read_word( CALIBRATE_AZIMUTH );
	
	// Timer for reading position feedback
	Timer_SetTimeout(0, 100, TimerTypeFreeRunning, 0);
	Timer_SetTimeout(1, 1000, TimerTypeFreeRunning, 0);

	sendStatus( STATUS );

	while (1) {
		if (Timer_Expired(0)) {
			// Periodicly read antennas position
			readFeedback();
		}
		if (Timer_Expired(1)) {
			sendStatus(STATUS);
		}
		
		if (rxMsgFull) {
			switch (rxMsg.Id){
				case MSG_CAL_SET: // Set calibration value
					if( 2 == rxMsg.DataLength ){
						calibration( SET, rxMsg.Data.words[0] );
					}
					break;
					
				case MSG_CAL_GET: // Get calibration value
					if( 0 == rxMsg.DataLength ){
						txMsg.Id = MSG_CAL_RET; 
						txMsg.DataLength = 2;
						txMsg.Data.words[0] = calibration( GET, 0 );
						BIOS_CanSend(&txMsg);
					}
					break;
					
				case MSG_ABS: // Start turning to absolute position
					if( 2 == rxMsg.DataLength ){
						
					}
					break;
					
				case MSG_REL: // Start turning to relative position
					if( 2 == rxMsg.DataLength ){
						
					}
					break;
					
				case MSG_START: // Start turning
					if( 1 == rxMsg.DataLength ){
						// First data byte decides direction
						controlRelay( rxMsg.Data.bytes[0] );
					}
					break;
					
				case MSG_STOP: // Stop turning
					//if( 1 == rxMsg.DataLength ){
						controlRelay( ROTATE_STOP );
					//}
					break;
				case MSG_STATUS: // Get position
					if( 0 == rxMsg.DataLength ){
						sendStatus(STATUS);
					}
					break;
					
				default:
					break;
			}

    			rxMsgFull = 0; //  
		}
	}
	
	return 0;
}