//解锁
rt_err_t disarm()
{
Motor_Set(0, 0, 0, 0);
if (!armed)
return RT_EOK;
armed = RT_FALSE;
rt_kprintf("disarmed.\n");
excute_task("wait");
return RT_EOK;
}
void alarm_handler(int s)
{
excute_task();
timers_reboot();
}
//控制线程循环
void control_thread_entry(void *parameter)
{
LED2(5);
wait_dmp();
rt_kprintf("start control\n");
excute_task("wait");
while (1)
{
LED2(armed * 3);
receive_pwm(&pwm); //接受PWM信号
//若遥控器关闭则锁定
if (pwm.switch1 < 2 && pwm.switch1 != -1 && pwm.throttle < 0.05f && pwm.throttle >= 0.0f)
{
arm(SAFE_PWM);
}
/*
符合解锁条件则解锁
1.遥控器开关处于2档或3档 或者 两根遥控器屏蔽跳线接地
2.任务安全需求满足
*/
if ((((pwm.switch1 == 2 || pwm.switch1 == -1) && (GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_3) == Bit_RESET || GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_4) == Bit_RESET)) || check_safe(current_task->depend)))
{
disarm();
}
//得到dmp数据且解锁则执行任务
if (get_dmp() && armed)
{
//若地面站离线 遥控器在线则手动切换模式
if (check_safe(SAFE_PWM) == RT_EOK && check_safe(SAFE_TFCR) != RT_EOK)
{
if (current_task->id != 2 && pwm.switch1 == 1)
{
excute_task("stable");
}
if (current_task->id != 3 && pwm.switch1 == 0 && pwm.switch2 == 0)
{
excute_task("althold");
}
if (current_task->id != 4 && pwm.switch1 == 0 && pwm.switch2 == 1)
{
pos_X = ahrs.x;
pos_y = ahrs.y;
excute_task("loiter");
}
}
//执行任务循环
if (current_task->func(current_task->var) != RT_EOK)
disarm();
}
//若锁定则电机关闭
if (!armed)
Motor_Set(0, 0, 0, 0);
rt_sem_release(&watchdog);
rt_thread_delay(2);
}
}
void remote_thread_entry(void* parameter)
{
char * ptr;
u8 lost=0;
rt_tick_t last=rt_tick_get();
while(1)
{
ptr=pack.buf;
get(ptr++);
if(pack.buf[0]!=(head&0xFF))
goto wrong;
get(ptr++);
if(pack.head.head!=head)
goto wrong;
while(ptr-pack.buf<sizeof(struct tfcr_common))
get(ptr++);
if(!TFCR_IS_PACK_TYPE(pack.head.type))
goto wrong;
//rt_kprintf("head ok %d\n",sizeof(struct tfcr_common));
switch(pack.head.type)
{
case TFCR_TYPE_PING:
while(ptr-pack.buf<sizeof(struct tfcr_ping))
get(ptr++);
if(checksum(pack.buf,sizeof(struct tfcr_ping)-1)!=pack.ping.checksum)
goto wrong;
send_ack(pack.ping.index);
debug("ping %05d\n",pack.ping.index);
if(!tfrc_con)
rt_kprintf("tfcr connected.\n");
tfrc_con=RT_TRUE;
break;
case TFCR_TYPE_TASK:
while(ptr-pack.buf<sizeof(struct tfcr_task))
get(ptr++);
if(checksum(pack.buf,sizeof(struct tfcr_task)-1)!=pack.task.checksum)
goto wrong;
if(rt_strcasecmp(pack.task.name,"mayday")==0)
{
excute_task("mayday");
disarm();
send_ack(pack.task.index);
break;
}
if(pack.task.state==1)
{
fc_task * task=find_task(pack.task.name);
if(task==RT_NULL)
{
send_error(pack.task.index,0xff);
rt_kprintf("no task %s!\n",pack.task.name);
break;
}
u8 result= check_safe(task->depend);
if(result!=0)
{
send_error(pack.task.index,result);
break;
}
if(!excute_task(pack.task.name))
{
send_error(pack.task.index,0xff);
break;
}
arm(task->depend);
send_ack(pack.task.index);
}
else
{
excute_task("wait");
send_ack(pack.task.index);
rt_kprintf("stop task %s.\n",pack.task.name);
}
break;
case TFCR_TYPE_GET_VALUE:
while(ptr-pack.buf<sizeof(struct tfcr_get_value))
get(ptr++);
if(checksum(pack.buf,sizeof(struct tfcr_get_value)-1)!=pack.get.checksum)
goto wrong;
send_value(pack.get.index,pack.get.id);
break;
}
last=rt_tick_get();
lost=0;
continue;
timeout:
if(tfrc_con)
{
if(lost<3)
{
rt_kprintf("tfcr time out.\n");
lost++;
}
else
{
rt_kprintf("tfcr lost connect.\n");
tfrc_con=RT_FALSE;
}
}
continue;
wrong:
rt_kprintf("wrong\n");
if(rt_tick_get()-RT_TICK_PER_SECOND/2>last)
{
rt_tick_t last=rt_tick_get();
goto timeout;
}
}
}
