예제 #1
0
void main()
{
	uart_init(9600);
	vt_init();

	sonar_t so;
	sonar_t so2;

	sonar_init(&so, A0, A1);
	sonar_init(&so2, A2, A3);

	sei();

	// end
	int16_t res;
	while(1) {
		vt_clear();
		vt_home();
		uart_puts_pgm(PSTR("SONAR\r\n==================="));

		// first
		vt_goto(99, 99); // move cursor away
		sonar_start(&so);
		while(sonar_busy());
		res = sonar_result();

		// print value
		vt_goto(0, 2);
		uart_puts_pgm(PSTR("A: "));

		if (res < 0) {
			uart_puts_pgm(PSTR("No Obstacle"));
		} else {
			uart_puti(res, 1);
			uart_puts_pgm(PSTR(" cm"));
		}

		// second
		vt_goto(99, 99); // move cursor away
		sonar_start(&so2);
		while(sonar_busy());
		res = sonar_result();

		// print value
		vt_goto(0, 3);
		uart_puts_pgm(PSTR("B: "));

		if (res < 0) {
			uart_puts_pgm(PSTR("No Obstacle"));
		} else {
			uart_puti(res, 1);
			uart_puts_pgm(PSTR(" cm"));
		}

		vt_goto(99, 99); // move cursor away
		_delay_ms(200);
	}
}
예제 #2
0
파일: main.c 프로젝트: D3add3d/avr-lib
void main()
{
	lcd_init();

	// Init sonar
	sonar_t so;
	sonar_init(&so, A0, A1);

	sei();


	while(1) {
		// Measure
		sonar_start(&so);
		while(sonar_busy);

		int16_t res = sonar_result;

		// Print
		lcd_clear();
		if (sonar_result < 0) {
			put_str(lcd, "---");
		} else {
			put_i16f(lcd, res, 1); // one decimal place
			put_str(lcd, " cm");
		}

		// a delay...
		_delay_ms(200);
	}
}
예제 #3
0
int main(void)
{
	struct heading result;
	/*u08 k=0;
	u08 b=0;
	u08 choice=0;
	u08 button=0;*/
	u08 ir=0;
	u08 i=0;

	x = 64;
	y = 48;
	initialize();
	motor_init();
	servo_init();
	set_motor_power(0,0);
	set_motor_power(1,0);
	set_motor_power(2,0);
	set_motor_power(3,0);
	compass_init();
	sonar_init();

	calCompass();
	while(1)
	{
		clear_screen();
		result = compass();

		/* use calibration data */
		result.x -= compZero.x;
		result.y -= compZero.y;

		print_string("x ");     /*  2 */
		print_int(result.x);    /*  3 */
		print_string(" y ");    /*  3 */
		print_int(result.y);    /*  3 */
		/* print_string(" s ");  */ /*  3 */
		/* print_int(getSonar(0)); */ /*  3 */
					 /*  =17 */
		next_line();
		/* print_string("a "); */ /*  2 */
		/* print_int(IR(0)); */ /*  3 */
		/* print_string(" b "); */ /*  3 */
		/* print_int(analog(1)); */ /*  3 */
		/* print_string(" c "); */ /*  3 */
		/* print_int(analog(2)); */ /*  3 */
		/* print_int(i++); */ /*  =17 */
		/* print_int(distance(0));
		print_string(" "); */
		print_int(sizeof(int));
		delay_ms(200);
		/* print_int(i);
		i=sweep(); */

	}
}
예제 #4
0
 int	
 main(void){
    cli();
    CLOCK_8MHZ();
    DDRD  = 0xff;
    PORTD = 0xff;  // Turn on both LEDs
    uart_init();
    sonar_init();
    sei();

    uart_write((uint8_t*)"UART\r\n", 6);

    trigger_sonar();
    for(;;);

 }
예제 #5
0
void    custom_init(void)

{
    int     c;

    /*
     *  Below are custom configurations for your robot.  This is where
     *  you set up digital output ports, analog ports, sensors, etc.
     */
     
    /* Set some of the 16 I/O ports as inputs */
    for (c = 1; c <= 16; ++c)
	io_set_direction(c, IO_DIRECTION_IN);

    /*
     *  Configure the number of analog input ports. Ports 1 through N
     *  will be analog, and N+1 through 16 will be digital.
     */
    io_set_analog_port_count(ANALOG_PORTS);

    /* Set some of the 16 I/O ports as outputs (digital output only). */
    /* These can be used to drive LEDs and other low-current devices. */
    //for (c = 9; c <= 16; ++c)
    //    io_set_direction(c, IO_DIRECTION_OUT);

    /* Initialize the values on the digital outputs. */
    //for (c = 9; c <= 16; ++c)
    //    io_set_digital(c,0);

    /* Enable shaft encoder on any interrupt port */
    /* The count can be read back with shaft_encoder_read_count() */
    shaft_encoder_enable_std(LEFT_ENCODER_INTERRUPT_PORT);
    shaft_encoder_enable_std(RIGHT_ENCODER_INTERRUPT_PORT);
    
    /* Example: Quad encoder requires two pots */
    // quad_encoder_enable(6, 8);
    
    /* Enable sonar device and timer. */
    if ( sonar_init(SONAR_INTERRUPT_PORT, SONAR_OUTPUT_PORT) != OV_OK )
	printf("sonar_init() failed.\n");

    timer_start(1);
    
    controller_print_version();
}
예제 #6
0
파일: tests.c 프로젝트: aore/mars-rover
void readings_stream_mode()
{
	const uint8_t BUF_LENGTH = 100;
	char buf[BUF_LENGTH];

	enum {
		reading_ir = 1,
		reading_sonar = 2,
		reading_sonar_mode = 3
	} reading;

	init_push_buttons();
	lcd_init();

	ir_init();
	sonar_init();

	usart_init();
	usart_drain_rx();

	while (true) {
		switch (wait_button("Readings Stream")) {
		case reading_ir:
			snprintf(buf, BUF_LENGTH, "IR: %u\n", ir_raw_reading());
			break;

		case reading_sonar:
			snprintf(buf, BUF_LENGTH, "Sonar: %u\n", sonar_raw_reading());
			break;

		case reading_sonar_mode:
			while (true) {
				snprintf(buf, BUF_LENGTH, "Sonar: %u\n", sonar_raw_reading());
				usart_tx_buf(buf);
			}
			break;

		default:
			buf[0] = '\0';
		}

		usart_tx_buf(buf);
	}
}
예제 #7
0
int
main(int argc, char *argv[])
{
    motor_t rightMotor = {
        &PORTB, &DDRB, PORTB7, // PWM
        &PORTC, &DDRC, PORTC1, // Direction
        &OCR0A                 // Top value
    };
    
    motor_t leftMotor = {
        &PORTD, &DDRD, PORTD0, // PWM
        &PORTB, &DDRB, PORTB0, // Direction
        &OCR0B                 // Top value
    };
    
    //uint8_t sonarDistance = 0;
    
    cli();
    NO_CLK_PRESCALE();
    //uart_init();
    radio_init(HOV1_ADDRESS, RECEIVE_MODE);
    sonar_init();
    motorInit(&rightMotor);
    motorInit(&leftMotor);
    pwmInit();
    sei();
    
	stop = Event_Init();

    sendPing();
    while(!receivedInit);

    Task_Create((void*)(&motor_task),0, PERIODIC, MOTOR);
    Task_Create((void*)(&fire_sonar),0, PERIODIC, FIRE);
    Task_Create((void*)(&listen_sonar),0,PERIODIC, LISTEN);
    Task_Create((void*)(&sendRadio),0,PERIODIC, RADIO);
	Task_Create((void*)(&stopSystem), 0,SYSTEM, STOP); 
   
    setMotorDirection(&rightMotor, FORWARD);
    setMotorDirection(&leftMotor, FORWARD);
    
    return 0;
}
예제 #8
0
int main() {
    board_init();
    uart_init();
    esc_init();
    rgbled_init();
    rgbled_set(0xFF8000, 100);
    spektrum_init();
    rgbled_set(0xFF8000, 100);
    xbee_init();
    i2c_init();
    alt_init();
    mag_init();
    mpu_init();
    sonar_init();
    ins_init();
    inscomp_init();
    altitude_init();
    controller_init();
    basestation_init();
    flight_init();

    controlpanel_run();
}
예제 #9
0
int main()
{
	// string buffer for printing to UART
	uint8_t output[128];

	// disable the clock prescaler
	clock8MHz();

	// disable interrupts during setup
	cli();

	uart_init(UART_38400);
	sonar_init();

	// enable interrupts
	sei();


	for (;;)
	{
		sonar_trigger();
		_delay_ms(36);
		if (sonar_echo_received() == 1) {
			distance = 0.0171 * sonar_get_distance() -  0.8192;
			snprintf((char*)output, 128, "distance %d\n\r", (int)distance);
			print(output);
		}else {
			snprintf((char*)output, 128, "sonar failed\n\r");
			print(output);
		}

		_delay_ms(1000);
	}

	return 0;
}
예제 #10
0
int main(void)
{
	uint16_t counter;
	char c;
	uint8_t stop = TRUE;

	usart_init();
	sonar_init();
	rtc_setup();

	counter = 0;
	usart_resume(0);
	strcpy_P(usart->tx0_buffer, PSTR("\nTsunami Simulator "));
	strcat_P(usart->tx0_buffer, PSTR(GITREL));
	strcat_P(usart->tx0_buffer, PSTR("\n\nConnected!\n"));
	usart_printstr(0, NULL);
	rtc_start();

	while (1) {
		/* Restart the counter */
		rtc_clear();
		c = usart_getchar(0, FALSE);

		switch (c) {
			case '0':
				stop = TRUE;
				break;
			case '1':
				stop = FALSE;
				break;
			default:
				break;
		}

		if (stop) {
			/* stop the code */
			while(usart_getchar(0, FALSE) != '1');
			stop = FALSE;
			rtc_clear();
		}

		/* send the trigger */
		sonar_trigger();
		/* clear all the data */
		sonar_clear();

		/*
		 * Wait 40mS maximum and collect all the
		 * data during the period.
		 */
		while (rtc_us < 4000)
			sonar_set();

		/*
		 * speed = ((i * SCALEuS)/1000000) * 340 / 2
		 * where:
		 * i * SCALEuS is the duration in uS of the signal.
		 * (i * SCALEuS)/1000000 is the same in seconds.
		 * 340 is the speed of the sound and
		 * /2 we need only half of the way.
		 * The simplyfied formula in cm.
		 * 340 mm/msec = 34cm/msec = 0.029 msec/cm = 29 uS/cm
		 * dist (cm) = T (uS) / 29 /2.
		 */

		usart->tx0_buffer = utoa(counter, usart->tx0_buffer, 10);
		usart_printstr(0, NULL);
		usart_printstr(0, " ");
		counter++;
		sonar_print();

		/* if the counter has already reach 50mS,
		 * then this cycle takes too long.
		 */
		if (rtc_us > 5000)
			usart_printstr(0, "Warning! Time overrun.\n");

		/* Wait up to 50mS before restart */
		while (rtc_us < 5000);
	}

	return(0);
}
예제 #11
0
int main(void)
{
	initialize();
	servo_init();
	motor_init();
	sonar_init();
	set_servo_position(0, 120); //center the servo
	
	print_string("Rodentia Demo");
	next_line();
	print_string("by Austin");
	
	led_on();
	delay_ms(100);
	led_off();
	delay_ms(100);
	led_on();
	delay_ms(100);
	led_off();
	
	//wait for a start signal
	while(!get_sw1())
	{
		/*u08 temp;
		clear_screen();
		temp = analog(0);
		print_int(temp);*/
		delay_ms(50);
	}
	led_on();
	
	while(1)
	{ //find and point toward heading with minimum sonar reading
		u08 heading = 0;
		/*u16 minHeading = 0;
		u16 left = 0;
		u08 aLeft = 1;
		
		u16 right = 0;
		u08 aRight = 1;*/
		u16 sonar = 0x0;
		u16 temp = 0;
		set_servo_position(0, 120); //center the servo
		set_motor_power(0, MIN_POWER);
		set_motor_power(1, -MIN_POWER);
		while(heading < 170) //what's the magic number?
		{
			/*u08 tempL = analog(1);
			u08 tempR = analog(2);
			if(tempL < 25 && aLeft)
			{
				set_motor_power(0, 0);
				left++;
				aLeft = 0;
			}
			if(tempL > 180 && !aLeft)
			{
				set_motor_power(0, 0);
				left++;
				aLeft =  1;
			}
			if(tempR < 25 && aRight)
			{
				set_motor_power(1, 0);
				right++;
				aRight = 0;
			}
			if(tempR > 180 && !aRight)
			{
				set_motor_power(1, 0);
				right++;
				aRight =  1;
			}
			if(left > heading && right > heading)*/
			{
				heading++;
				temp = getSonar(0);
				if(IR(0) > 70)
				{
					temp = 0;
				}
				if(temp > sonar)
				{
					sonar = temp;
					//minHeading = heading;
				}
				clear_screen();
				print_string("here=");
				print_int(temp);
				print_string(" max=");
				print_int(sonar);
				/*next_line();
				print_string("Hdng=");
				print_int(heading);
				print_string(" minH=");
				print_int(minHeading);
				if(left < right)
				{ //left motor on
					set_motor_power(0, MIN_POWER);
				}
				else if (right < left)
				{ //right motor on
					set_motor_power(1, -MIN_POWER);
				}
				else
				{
					set_motor_power(0, MIN_POWER);
					set_motor_power(1, -MIN_POWER);
				}*/
			}
		}
		
		if(sonar > 140)
		{ //set a reasonable maximum value for sonar
			sonar = 140;
		}
		
		//turn back to minumum heading
		temp = 0;
		while( temp < sonar )
		{
			temp = getSonar(0) + 3;
			if(IR(0) > 70)
			{
				temp = 0;
			}
			clear_screen();
			print_string("here=");
			print_int(temp);
			print_string(" max=");
			print_int(sonar);
		}
		/*set_motor_power(0, -MIN_POWER);
		set_motor_power(1, MIN_POWER);
		while(heading > minHeading)
		{
			u08 tempL = analog(1);
			u08 tempR = analog(2);
			if(tempL < 25 && aLeft)
			{
				set_motor_power(0, 0);
				left--;
				aLeft = 0;
			}
			if(tempL > 180 && !aLeft)
			{
				set_motor_power(0, 0);
				left--;
				aLeft =  1;
			}
			if(tempR < 25 && aRight)
			{
				set_motor_power(1, 0);
				right--;
				aRight = 0;
			}
			if(tempR > 180 && !aRight)
			{
				set_motor_power(1, 0);
				right--;
				aRight =  1;
			}
			if(left < heading && right < heading)
			{
				heading--;
				clear_screen();
				print_string("L=");
				print_int(left);
				print_string(" R=");
				print_int(right);
				next_line();
				print_string("Hdng=");
				print_int(heading);
				print_string(" minH=");
				print_int(minHeading);
				if(left > right)
				{ //left motor on
					set_motor_power(0, -MIN_POWER);
				}
				else if (right > left)
				{ //right motor on
					set_motor_power(1, MIN_POWER);
				}
				else
				{
					set_motor_power(0, -MIN_POWER);
					set_motor_power(1, MIN_POWER);
				}
			}
		}*/
		set_motor_power(0, 0);
		set_motor_power(1, 0);
		
		//scan with IR for obstacles
		delay_ms(500);
		u08 dir=120;
		s08 dDir = 1;
		u08 dist = IR(0);
		set_motor_power(0, 100);
		set_motor_power(1, 100);
		while(dist < 100)
		{
			dir += dDir;
			set_servo_position(0, dir);
			if(dir >= 200)
			{
				dDir = -1;
			}
			if(dir <= 40)
			{
				dDir = 1;
			}
			dist = IR(0);
			clear_screen();
			print_string("Distance=");
			print_int(dist);
		}
	}
	/*u08 temp;
	
	while(1)
	{
		clear_screen();
		temp = analog(0);
		print_int(temp);
		set_servo_position(0,temp);
		delay_ms(50);
	}*/
}
예제 #12
0
void init_state() {
    static int first = 1;
    int retval;

    if(first) {
        
        if(debug)
            spawn_debug_thread();

#ifdef USE_KILL_SWITCH
        //initialize kill switch
        retval = switch_init();
        if(retval != SWITCH_NO_ERROR) {
            snprintf(state_data.error_str, sizeof(state_data.error_str), SWITCH_ERROR_STR(retval));
            next_state = ERROR_STATE;
            return;
        }
       kill_switch_initialized = 1;
#endif

#ifdef USE_GPS
        // initialize gps 
        retval = gps_init();
        if(retval != GPS_NO_ERROR) {
            snprintf(state_data.error_str, sizeof(state_data.error_str), GPS_ERROR_STR(retval));
            next_state = ERROR_STATE;
            return;
        }
       gps_initialized = 1;
#endif

#ifdef USE_SONAR
        // initialize sonar sensors
        retval = sonar_init();
        if(retval != SONAR_NO_ERROR) {
            snprintf(state_data.error_str, sizeof(state_data.error_str), SONAR_ERROR_STR(retval));
            next_state = ERROR_STATE;
            return;
        }
       sonar_initialized = 1;
#endif

#ifdef USE_COMPASS
        //initialize compass
        retval = compass_init();
        if(retval != COMPASS_NO_ERROR) {
            snprintf(state_data.error_str, sizeof(state_data.error_str), SONAR_ERROR_STR(retval));
            next_state = ERROR_STATE;
            return;
        }
        compass_initialized = 1;
#endif

#ifdef USE_CAMERA
        //initialize camera
        retval = camera_init();
        if(retval != CAMERA_NO_ERROR) {
            snprintf(state_data.error_str, sizeof(state_data.error_str), CAMERA_ERROR_STR(retval));
            next_state = ERROR_STATE;
            return;
        }
        retval = camera_start_tracking();
        if(retval != CAMERA_NO_ERROR) {
            snprintf(state_data.error_str, sizeof(state_data.error_str), CAMERA_ERROR_STR(retval));
            next_state = ERROR_STATE;
            return;
        }
        camera_initialized = 1;
#endif
#ifdef USE_CAR
        retval = init_car();
        if(retval != CAR_NO_ERROR) {
            snprintf(state_data.error_str, sizeof(state_data.error_str), CAR_ERROR_STR(retval));
            next_state = ERROR_STATE;
            return;
        }
        car_initialized = 1;
#endif

        spawn_device_threads();
        first = 0;
    }

    if (kill_switch_asserted)
        next_state = PAUSE_STATE;
    else
        next_state = NAVIGATION_STATE;
    //next_state = TRACK_STATE;
}
예제 #13
0
int threads_linux_init(void)
{
    int status = 0;
    int n = 0;
    int return_value = THREADS_LINUX_SUCCESS;

    // Init data
    status = sensors_init(&battery_data, &gps_data, &imu_data, &pitot_data, &pwm_read_data, &pwm_write_data, &scp1000_data, &sonar_data);
    if(status != SENSORS_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = calibration_init(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
        if(status != CALIBRATION_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = gps_init(&gps_measure);
        if(status != 1)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = imu_init(&imu_measure);
        if(status != 1)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = magnetometer_init(&magnetometer_measure);
        if(status != 1)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
    	status = sonar_init(&sonar_measure);
    	if(status != 1)
    	{
    		return_value = THREADS_LINUX_FAILURE;
    	}
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = estimation_init(ESTIMATION_DO_NOT_ESTIMATE_ACCEL_BIAS, &estimation_data);
        if(status != ESTIMATION_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = control_init(&control_data);
        if(status != CONTROL_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = protocol_init();
        if(status != PROTOCOL_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = ui_init();
        if(status != UI_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = datalogger_init();
        if(status != DATALOGGER_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    // Main Loop
    if(return_value != THREADS_LINUX_FAILURE)
    {
        threads_linux_timer_start_task_1();
        usleep(0.5*task_1_period_us);
        threads_linux_timer_start_task_2();
        usleep(5*task_1_period_us);

        while(quittask == 0)
        {
            #if ANU_COMPILE_FOR_OVERO
            #else
            if(datalogger_status() == DATALOGGER_RUNNING) datalogger_update_IPC();
            #endif
            if(++n>100)
            {
                if(datalogger_status() == DATALOGGER_RUNNING) datalogger_write_file();
                n = 0;
            }
            usleep(5*task_1_period_us);
        }

        threads_linux_timer_stop_task_1();
        usleep(TASK1_PERIOD_US);
        threads_linux_timer_stop_task_2();
        usleep(TASK2_PERIOD_US);
    }

    status = datalogger_close();
    if(status != DATALOGGER_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = ui_close();
    if(status != UI_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = protocol_close();
    if(status != PROTOCOL_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = control_close();
    if(status != CONTROL_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = estimation_close(&estimation_data);
    if(status != ESTIMATION_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = magnetometer_close(&magnetometer_measure);
    if(status != 1)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = imu_close(&imu_measure);
    if(status != 1)
    {
        return_value = THREADS_LINUX_FAILURE;
    }
    status = gps_close(&gps_measure);
    if(status != 1)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = calibration_close(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
    if(status != CALIBRATION_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = sensors_close();
    if(status != SENSORS_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    return return_value;
}
예제 #14
0
void rt_init_thread_entry(void* parameter)
{
	rt_components_init();

	LED_init();
	Motor_Init();

	rt_kprintf("start device init\n");

	//rt_hw_i2c1_init();
	i2cInit();
	rt_hw_spi2_init();
	rt_hw_spi3_init();

	rt_event_init(&ahrs_event, "ahrs", RT_IPC_FLAG_FIFO);

	dmp_init();
	sonar_init();
	HMC5983_Init();
	adns3080_Init();

	//config_bt();

	rt_thread_init(&led_thread,
		"led",
		led_thread_entry,
		RT_NULL,
		led_stack,
		256, 16, 1);
	rt_thread_startup(&led_thread);

	spi_flash_init();

	//	bmp085_init("i2c1");

	rt_kprintf("device init succeed\n");

	if (dfs_mount("flash0", "/", "elm", 0, 0) == 0)
	{
		rt_kprintf("flash0 mount to /.\n");
	}
	else
	{
		rt_kprintf("flash0 mount to / failed.\n");
	}

	//default settings
	PID_Init(&p_rate_pid, 0, 0, 0);
	PID_Init(&r_rate_pid, 0, 0, 0);
	PID_Init(&y_rate_pid, 0, 0, 0);
	PID_Init(&p_angle_pid, 0, 0, 0);
	PID_Init(&r_angle_pid, 0, 0, 0);
	PID_Init(&y_angle_pid, 0, 0, 0);
	PID_Init(&x_v_pid, 0, 0, 0);
	PID_Init(&y_v_pid, 0, 0, 0);
	PID_Init(&x_d_pid, 0, 0, 0);
	PID_Init(&y_d_pid, 0, 0, 0);
	PID_Init(&h_pid, 0, 0, 0);

	load_settings(&settings, "/setting", &p_angle_pid, &p_rate_pid
		, &r_angle_pid, &r_rate_pid
		, &y_angle_pid, &y_rate_pid
		, &x_d_pid, &x_v_pid
		, &y_d_pid, &y_v_pid
		, &h_pid);

	settings.roll_min = settings.pitch_min = settings.yaw_min = 1000;
	settings.th_min = 1000;
	settings.roll_max = settings.pitch_max = settings.yaw_max = 2000;
	settings.th_max = 2000;

	//	if(settings.pwm_init_mode)
	//	{
	//		Motor_Set(1000,1000,1000,1000);
	//
	//		rt_thread_delay(RT_TICK_PER_SECOND*5);
	//
	//		Motor_Set(0,0,0,0);
	//
	//		settings.pwm_init_mode=0;
	//		save_settings(&settings,"/setting");
	//
	//		rt_kprintf("pwm init finished!\n");
	//	}

	get_pid();
	PID_Set_Filt_Alpha(&p_rate_pid, 1.0 / 166.0, 20.0);
	PID_Set_Filt_Alpha(&r_rate_pid, 1.0 / 166.0, 20.0);
	PID_Set_Filt_Alpha(&y_rate_pid, 1.0 / 166.0, 20.0);
	PID_Set_Filt_Alpha(&p_angle_pid, 1.0 / 166.0, 20.0);
	PID_Set_Filt_Alpha(&r_angle_pid, 1.0 / 166.0, 20.0);
	PID_Set_Filt_Alpha(&y_angle_pid, 1.0 / 75.0, 20.0);
	PID_Set_Filt_Alpha(&x_v_pid, 1.0 / 100.0, 20.0);
	PID_Set_Filt_Alpha(&y_v_pid, 1.0 / 100.0, 20.0);
	PID_Set_Filt_Alpha(&x_d_pid, 1.0 / 100.0, 20.0);
	PID_Set_Filt_Alpha(&y_d_pid, 1.0 / 100.0, 20.0);
	PID_Set_Filt_Alpha(&h_pid, 1.0 / 60.0, 20.0);

	rt_thread_init(&control_thread,
		"control",
		control_thread_entry,
		RT_NULL,
		control_stack,
		1024, 3, 5);
	rt_thread_startup(&control_thread);

	rt_thread_init(&correct_thread,
		"correct",
		correct_thread_entry,
		RT_NULL,
		correct_stack,
		1024, 12, 1);
	rt_thread_startup(&correct_thread);

	LED1(5);
}