void rt_platform_init(void)
{
#ifdef RT_USING_SPI
rt_hw_spi2_init();
rt_hw_spi1_init();
#endif /* RT_USING_SPI */
// rt_mems_hw_init("MOTION") ;
rt_device_init_all();
}
void rt_init_thread_entry(void *parameter)
{
#ifdef RT_USING_COMPONENTS_INIT
/* initialization RT-Thread Components */
rt_components_init();
#endif
#ifdef RT_USING_SPI
rt_hw_spi2_init();
#endif
}
void rt_platform_init(void)
{
#ifdef RT_USING_SPI
rt_hw_spi2_init();
#ifdef RT_USING_DFS
w25qxx_init("flash0", "spi20");
#endif /* RT_USING_DFS */
#ifdef RT_USING_RTGUI
/* initilize touch panel */
rtgui_touch_hw_init("spi21");
#endif /* RT_USING_RTGUI */
#endif /* RT_USING_SPI */
#ifdef RT_USING_USB_HOST
/* register stm32 usb host controller driver */
rt_hw_susb_init();
#endif
#ifdef RT_USING_DFS
/* initilize sd card */
#ifdef RT_USING_SDIO
rt_mmcsd_core_init();
rt_mmcsd_blk_init();
stm32f4xx_sdio_init();
rt_thread_delay(RT_TICK_PER_SECOND);
#else
rt_hw_sdcard_init();
#endif
#endif /* RT_USING_DFS */
#ifdef RT_USING_RTGUI
/* initilize ra8875 lcd controller */
ra8875_init();
/* initilize key module */
rt_hw_key_init();
#endif /* RT_USING_RTGUI */
#ifdef RT_USING_RTC
rt_hw_rtc_init();
#endif /* RT_USING_RTC */
rt_thread_delay(50);
rt_device_init_all();
}
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);
}
