//锁定
rt_err_t arm(rt_int32_t addtion)
{
rt_err_t err;
if (armed)
return RT_EOK;
PID_Reset(&p_angle_pid);
PID_Reset(&p_rate_pid);
PID_Reset(&r_angle_pid);
PID_Reset(&r_rate_pid);
PID_Reset(&y_angle_pid);
PID_Reset(&y_rate_pid);
PID_Reset(&h_v_pid);
PID_Reset(&h_d_pid);
PID_Reset(&x_d_pid);
PID_Reset(&x_v_pid);
PID_Reset(&y_d_pid);
PID_Reset(&y_v_pid);
if ((err = check_safe(SAFE_MPU6050 | addtion)) == RT_EOK)
{
yaw_exp = ahrs.degree_yaw;
armed = RT_TRUE;
rt_kprintf("armed.\n");
return RT_EOK;
}
else
{
rt_kprintf("pre armed fail: 0x%02X.\n", err);
return err;
}
}
//光流定点算法
void loiter(float x, float y, float yaw)
{
rt_uint32_t dump;
if (check_safe(SAFE_ADNS3080))
{
x_v_pid.out = 0;
y_v_pid.out = 0;
return;
}
if (rt_event_recv(&ahrs_event, AHRS_EVENT_ADNS3080, RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_NO, &dump) == RT_EOK)
{
PID_SetTarget(&x_d_pid, pos_X);
PID_SetTarget(&y_d_pid, pos_y);
PID_xUpdate(&x_d_pid, ahrs.x);
PID_xUpdate(&y_d_pid, ahrs.y);
PID_SetTarget(&x_v_pid, -RangeValue(x_d_pid.out, -10, 10));
PID_SetTarget(&y_v_pid, -RangeValue(y_d_pid.out, -10, 10));
PID_xUpdate(&x_v_pid, ahrs.dx);
PID_xUpdate(&y_v_pid, ahrs.dy);
}
stable(+RangeValue(y_v_pid.out, -10, 10), -RangeValue(x_v_pid.out, -10, 10), yaw);
}
//执行任务
rt_bool_t excute_task(const char *name)
{
fc_task *t = find_task(name);
if (t == RT_NULL)
return RT_FALSE;
if (check_safe(t->depend))
{
rt_kprintf("start task %s fail %d.\n", t->name, check_safe(t->depend));
return RT_FALSE;
}
current_task = t;
current_task->reset = RT_TRUE;
rt_kprintf("start task %s.\n", t->name);
return RT_TRUE;
}
//光流定点模式
rt_err_t loiter_mode(u8 height)
{
const u16 basic_thought = 450;
if (check_safe(SAFE_ADNS3080))
return RT_EIO;
if (pwm.throttle > 0.3f && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40)
{
loiter(pos_X, pos_y, yaw_exp);
althold(height);
motor_hupdate(basic_thought);
}
else
Motor_Set(60, 60, 60, 60);
return RT_EOK;
}
//定高算法
void althold(float height)
{
rt_uint32_t dump;
if (check_safe(SAFE_SONAR))
{
h_v_pid.out = 0;
return;
}
//等待超声波模块信号
if (rt_event_recv(&ahrs_event, AHRS_EVENT_SONAR, RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_NO, &dump) == RT_EOK)
{
//双环定高
PID_SetTarget(&h_d_pid, 60.0f);
PID_xUpdate(&h_d_pid, ahrs.height);
PID_SetTarget(&h_v_pid, -RangeValue(h_d_pid.out, -50, +50));
PID_xUpdate(&h_v_pid, ahrs.height_v);
h_v_pid.out = RangeValue(h_v_pid.out, -300, 300);
}
}
int request_resources(int customer_num, int request[])
{
int iter,j,ii;
printf("\n %d customer request arrived ", customer_num);
for(iter=0;iter<NUMBER_OF_RESOURCES;iter++)
{
//request cannot be processed
if(request[iter] > need[customer_num][iter])
return -1;
}
//pretend to have allocated the requested resources to process Pi
for(iter=0;iter<NUMBER_OF_RESOURCES;iter++)
{
pthread_mutex_lock(&mutexalloc);
allocation[customer_num][iter] += request[iter];
pthread_mutex_unlock(&mutexalloc);
pthread_mutex_lock(&mutexavail);
available[iter] -= request[iter];
pthread_mutex_unlock(&mutexavail);
pthread_mutex_lock(&mutexneed);
need[customer_num][iter] -= request[iter];
pthread_mutex_unlock(&mutexneed);
}
printf("\n Available Matrix \n");
for(ii=1;ii<=NUMBER_OF_RESOURCES;ii++)
{
printf("%d ",available[ii-1]);
}
printf("\n");
print_matrix(allocation,"allocation");
print_matrix(need,"need");
print_matrix(maximum,"max");
//revert to old state if its not safe
if(check_safe()<0)
{
for(iter=0;iter<NUMBER_OF_RESOURCES;iter++)
{
pthread_mutex_lock(&mutexalloc);
allocation[customer_num][iter] -= request[iter];
pthread_mutex_unlock(&mutexalloc);
pthread_mutex_lock(&mutexavail);
available[iter] += request[iter];
pthread_mutex_unlock(&mutexavail);
pthread_mutex_lock(&mutexneed);
need[customer_num][iter] += request[iter];
pthread_mutex_unlock(&mutexneed);
}
return -1;
}
else
printf("\n Hurray! I am accepted %d",customer_num);
return 0;
}
//控制线程循环
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 DropletCustomOne::DropletMainLoop()
{
switch ( state )
{
case COLLABORATING:
if ( get_32bit_time() - collab_time > COLLABORATE_DURATION )
{
change_state ( LEAVING );
}
break;
case LEAVING:
if ( !is_moving(NULL) )
{
change_state ( SAFE );
}
break;
case SAFE:
if ( check_safe () )
{
change_state ( SEARCHING );
}
// If you start in the red region then try to get out before you die
else
{
random_walk ();
}
break;
case SEARCHING:
random_walk ();
if ( !check_safe() )
{
change_state ( WAITING );
}
break;
case START_DELAY:
{
uint32_t curr_time = get_32bit_time ();
if ( curr_time - start_delay_time > START_DELAY_TIME )
change_state ( SAFE );
}
break;
case WAITING:
if ( get_32bit_time() - heartbeat_time > HEART_RATE )
{
heartbeat_time = get_32bit_time ();
send_heartbeat ();
}
// Checks incoming messages and updates group size.
// There is a chance the state can be changed to COLLABORATING in
// this function if the droplet sees a GO message.
update_group_size ();
if ( get_32bit_time() - voting_time > HEART_RATE &&
state == WAITING )
{
voting_time = get_32bit_time ();
check_votes ();
}
break;
default:
break;
}
}
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;
}
}
}
