/**
* 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());
}
//控制初始化
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();
}