/**
* System initialization thread.
*
* @param parameter parameter
*/
void sys_init_thread(void* parameter){
set_system_status(SYSTEM_STATUS_INIT);
/* EasyFlash initialization */
if (flash_init() == FLASH_NO_ERR) {
/* initialize OK and switch to running status */
set_system_status(SYSTEM_STATUS_RUN);
/* test Env demo */
test_env();
} else {
/* initialize fail and switch to fault status */
set_system_status(SYSTEM_STATUS_FAULT);
}
rt_thread_delete(rt_thread_self());
}
void control_cmd_confirm()
{
if (get_system_status() == SYS_PREPARE_SETTING) {
if (waypoint_is_ready && prepare_setting_is_ready)
set_system_status(SYS_PREPARE_STEERING_TEST);
//control_cmd_send(ack_response, 13);
} else {
print_err("control_cmd_confirm error: system status is %d\n", get_system_status());
}
}
void steering_test()
{
if(fread(&ppwm,20,1,fp_servo_test)==0){
servo_test_enable=0;
set_system_status(SYS_PREPARE_TAKEOFF);
fclose(fp_servo_test);
printf("servo test over\n");
}else{
write_pwm_data((uint16*)&ppwm);
}
}
/**
* 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());
}
int poweron_self_check()
{
int ret = -1;
int timeout = 0;
char set_system_time[50];
flying_attitude_s *p;
//--------------spi initial------------------
ret=spi_open();
printf("CPLD logic version:%d\n",get_fpga_version());
printf("----------------------------------------------------------------\n");
#ifndef debug
if(ret<0)
fault_status_return(ret);
#endif
/*
ret=adc_init();
#ifndef debug
if(ret<0)
fault_status_return(ret);
#endif
*/
//---------------imu initial-----------------
ret = sensor_open();
#ifndef debug
if (ret < 0) {
fault_status_return(ret);
}
#endif
pressure_sensor_init();
set_flying_status(AIRCRAFT_PREPARING);
if(command==0){
//UAV is working in normal mode
// reset CPLD ,this should be check later ,if CPLD should be reset when CPU recovers from failure.
reset_control_register(CTRL_REG_MASK_MANUAL);
// wait for flying attitude sensor data
while (timeout <= 600) {
sleep(1);
if(flying_attitude_sensor_is_active())
break;
timeout++;
}
if (timeout >600) {
print_err("flying attitude sensor is inactive, please check serial port is connected\n");
goto exit;
}
print_debug("Flying attitude sensor is active\n");
p=get_flying_attitude();
//printf("system beijing time %d-%d-%d,%d:%d:%d\n",p->year,p->month,p->day,p->hour+8,p->min,p->sec);
sprintf(set_system_time,"date -s \"%d-%d-%d %d:%d:%d\"",p->year+2000,p->month,p->day,p->hour+8,p->min,p->sec);
//printf("%s\n",set_system_time);
system(set_system_time);
}else if(command==1){
//UAV is working in test mode
reset_control_register(CTRL_REG_MASK_MANUAL);
printf("uav is working in test mode,IMU is by passed,using test data instead\n");
}else if(command==2){
//UAV is in mannual mode ,for pwm data capture
set_flying_status(AIRCRAFT_MANUAL_MODE);
set_control_register(CTRL_REG_MASK_MANUAL);
set_system_status(SYS_PREPARE_SETTING);
printf("UAV is now working in data capturing mode\n");
}else{
printf(" error command,please check the usage:\n");
printf(" usage: uav [command] [gap] ]\n");
printf(" [command]: 0--normal mode,used when run in the air\n");
printf(" [command]: 1--test mode,used for platform test.use this mode \n");
printf(" when no IMU connected ,\n");
printf(" [command]: 2--manual mode,used when capturing PWM data\n");
printf(" [frequency]: the frequency (ms/frame) in which UAV send fly status data\n");
}
generate_file_name(log_file_name);
if((fp_fly_status=fopen(log_file_name,"wb+"))==NULL){
printf("can not open file:%s\n",log_file_name);
}
heli_configuration_init();
control_parameter_init();
servo_test_enable=0;
while (1) {
if(get_system_status() < SYS_PREPARE_STEERING_TEST){
flying_status_return(1);
usleep(500000);
}else if(get_system_status() == SYS_PREPARE_STEERING_TEST){
steering_test();
usleep(20000);
}else if(get_system_status() == SYS_PREPARE_TAKEOFF){
return 0;
}
/*
if (get_system_status() >= SYS_PREPARE_SETTING) {
print_debug("Link is ready\n");
return 0;
}
link_testing_send();
usleep(20000); //20ms
*/
}
exit:
sensor_close();
return ret;
}
