Example #1
0
/**
 * System initialization thread.
 *
 * @param parameter parameter
 */
void sys_init_thread(void* parameter){
	set_system_status(SYSTEM_STATUS_INIT);

    /* EasyLogger initialization */
    if (elog_init() == ELOG_NO_ERR) {
        /* set enabled format */
        elog_set_fmt(ELOG_FMT_LVL | ELOG_FMT_TAG | ELOG_FMT_TIME /*| ELOG_FMT_P_INFO*/ | ELOG_FMT_T_INFO | ELOG_FMT_DIR
                /*| ELOG_FMT_FUNC*/ | ELOG_FMT_LINE);
        /* set EasyLogger assert hook */
        elog_assert_set_hook(elog_user_assert_hook);
        /* set hardware exception hook */
        rt_hw_exception_install(exception_hook);
        /* set RT-Thread assert hook */
        rt_assert_set_hook(rtt_user_assert_hook);
        /* initialize OK and switch to running status */
        set_system_status(SYSTEM_STATUS_RUN);
    } else {
        /* initialize fail and switch to fault status */
        set_system_status(SYSTEM_STATUS_FAULT);
    }

    rt_thread_delete(rt_thread_self());
}
Example #2
0
//控制初始化
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();
}