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