//控制初始化 void control_init() { GPIO_InitTypeDef gpio_init; GPIO_StructInit(&gpio_init); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); gpio_init.GPIO_Mode = GPIO_Mode_IN; gpio_init.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_3 | GPIO_Pin_4; gpio_init.GPIO_PuPd = GPIO_PuPd_DOWN; GPIO_Init(GPIOE, &gpio_init); //默认参数 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_v_pid, 0, 0, 0); PID_Init(&h_d_pid, 1.5f, 0, 0.8f); //加载PID参数 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_v_pid); //硬编码电机设置 settings.roll_min = settings.pitch_min = settings.yaw_min = 1017; settings.th_min = 1017; settings.roll_max = settings.pitch_max = settings.yaw_max = 2021; settings.th_max = 2021; settings.roll_mid = settings.pitch_mid = settings.yaw_mid = 1519; get_pid(); //打印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 / 166.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_v_pid, 1.0 / 60.0, 20.0); PID_Set_Filt_Alpha(&h_d_pid, 1.0 / 60.0, 20.0); h_v_pid.maxi = 150; rt_hw_exception_install(hardfalt_protect); rt_assert_set_hook(assert_protect); rt_sem_init(&watchdog, "watchdog", 0, RT_IPC_FLAG_FIFO); rt_thread_init(&control_thread, "control", control_thread_entry, RT_NULL, control_stack, 2048, 3, 5); rt_thread_startup(&control_thread); rt_thread_init(&watchdog_thread, "watchdog", watchdog_entry, RT_NULL, watchdog_stack, 512, 1, 1); rt_thread_startup(&watchdog_thread); extern void line_register(void); line_register(); }
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); }