//初始化等待DMP数据稳定
void wait_dmp()
{
u8 i;
for (i = 0; i < 15; i++)
{
u8 j;
for (j = 0; j < 200; j++)
{
get_dmp();
rt_sem_release(&watchdog);
rt_thread_delay(2);
}
}
ahrs.height_acc_fix = ahrs.height_acc;
}
//控制线程循环
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 control_thread_entry(void* parameter)
{
float yaw_inc = 0;
float yaw_exp = 0;
u8 i;
u8 take_off = 0;
p_rate_pid.expect = 0;
r_rate_pid.expect = 0;
y_rate_pid.expect = 0;
p_angle_pid.expect = 0;
r_angle_pid.expect = 0;
LED2(5);
for (i = 0;i < 15;i++)
{
u8 j;
for (j = 0;j < 200;j++)
{
get_dmp();
rt_thread_delay(2);
}
}
rt_kprintf("start control\n");
while (1)
{
LED2(pwmcon * 3);
if (pwmcon)
{
if (PWM3_Time <= settings.th_max&&PWM3_Time >= settings.th_min)
throttle = (PWM3_Time - settings.th_min) * 1000 / (settings.th_max - settings.th_min);
else
throttle = 0;
if (PWM1_Time <= settings.roll_max&&PWM1_Time >= settings.roll_min)
{
roll = MoveAve_WMA(PWM1_Time, roll_ctl, 16) - settings.roll_mid;
if (roll > 5)
PID_SetTarget(&r_angle_pid, -roll / (float)(settings.roll_max - settings.roll_mid)*45.0f);
else if (roll < -5)
PID_SetTarget(&r_angle_pid, -roll
/ (float)(settings.roll_mid - settings.roll_min)*45.0f);
else
PID_SetTarget(&r_angle_pid, 0);
}
if (PWM2_Time <= settings.pitch_max&&PWM2_Time >= settings.pitch_min)
{
pitch = MoveAve_WMA(PWM2_Time, pitch_ctl, 16) - settings.pitch_mid;
if (pitch > 5)
PID_SetTarget(&p_angle_pid, -pitch
/ (float)(settings.pitch_max - settings.pitch_mid)*30.0f);
else if (pitch < -5)
PID_SetTarget(&p_angle_pid, -pitch
/ (float)(settings.pitch_mid - settings.pitch_min)*30.0f);
else
PID_SetTarget(&p_angle_pid, 0);
}
if (PWM4_Time <= settings.yaw_max&&PWM4_Time >= settings.yaw_min)
{
yaw_inc = MoveAve_WMA(PWM4_Time, yaw_ctl, 16) - settings.yaw_mid;
if (has_hmc5883)
{
if (yaw_inc > 5)
yaw_exp -= yaw_inc / (float)(settings.yaw_max - settings.yaw_mid)*0.5f;
else if (yaw_inc < -5)
yaw_exp -= yaw_inc / (float)(settings.yaw_mid - settings.yaw_min)*0.5f;
if (yaw_exp > 360.0f)yaw_exp -= 360.0f;
else if (yaw_exp < 0.0f)yaw_exp += 360.0f;
PID_SetTarget(&y_angle_pid, 0);
}
else
{
if (yaw > 5)
PID_SetTarget(&y_rate_pid, -yaw_inc
/ (float)(settings.yaw_max - settings.yaw_mid)*100.0f);
else if (yaw < -5)
PID_SetTarget(&y_rate_pid, -yaw_inc
/ (float)(settings.yaw_mid - settings.yaw_min)*100.0f);
else
PID_SetTarget(&y_rate_pid, 0);
}
}
if (!balence)
Motor_Set(throttle, throttle, throttle, throttle);
}
else if (PWM3_Time > settings.th_min&&PWM3_Time < settings.th_min + 40 &&
PWM5_Time < 1700 && PWM5_Time>500)
{
//set pwm middle
if (!pwmcon)
{
settings.roll_mid = PWM1_Time;
settings.pitch_mid = PWM2_Time;
settings.yaw_mid = PWM4_Time;
}
pwmcon = 1;
balence = 1;
p_rate_pid.iv = 0;
r_rate_pid.iv = 0;
y_rate_pid.iv = 0;
take_off = 1;
yaw_exp = ahrs.degree_yaw;
}
if (get_dmp() && balence)
{
rt_uint32_t dump;
if (throttle > 60 && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40)
{
if (PWM5_Time < 1200 && PWM5_Time>800 && sonar_state)
{
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_pid, 60.0);
PID_xUpdate(&h_pid, sonar_h);
h_pid.out = RangeValue(h_pid.out, -200, 200);
}
LED4(4);
throttle = 500;
if (has_adns3080&& PWM7_Time > 1500)
{
if (!poscon)
{
poscon = 1;
pos_X = opx;
pos_y = opy;
}
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, opx);
PID_xUpdate(&y_d_pid, opy);
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, optc_dx);
PID_xUpdate(&y_v_pid, optc_dy);
}
LED4(2);
PID_SetTarget(&r_angle_pid, -RangeValue(x_v_pid.out, -10, 10));
PID_SetTarget(&p_angle_pid, +RangeValue(y_v_pid.out, -10, 10));
}
else
poscon = 0;
}
else
{
h_pid.out = 0;
LED4(0);
}
PID_xUpdate(&p_angle_pid, ahrs.degree_pitch);
PID_SetTarget(&p_rate_pid, -RangeValue(p_angle_pid.out, -80, 80));
PID_xUpdate(&p_rate_pid, ahrs.gryo_pitch);
PID_xUpdate(&r_angle_pid, ahrs.degree_roll);
PID_SetTarget(&r_rate_pid, -RangeValue(r_angle_pid.out, -80, 80));
PID_xUpdate(&r_rate_pid, ahrs.gryo_roll);
if (has_hmc5883)
{
if (rt_event_recv(&ahrs_event, AHRS_EVENT_HMC5883, RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_NO, &dump) == RT_EOK)
{
yaw = ahrs.degree_yaw - yaw_exp;
if (yaw > 180.0f)yaw -= 360.0f;
if (yaw < -180.0f)yaw += 360.0f;
PID_xUpdate(&y_angle_pid, yaw);
PID_SetTarget(&y_rate_pid, -RangeValue(y_angle_pid.out, -100, 100));
}
}
PID_xUpdate(&y_rate_pid, ahrs.gryo_yaw);
Motor_Set1(throttle - p_rate_pid.out - r_rate_pid.out + y_rate_pid.out - h_pid.out);
Motor_Set2(throttle - p_rate_pid.out + r_rate_pid.out - y_rate_pid.out - h_pid.out);
Motor_Set3(throttle + p_rate_pid.out - r_rate_pid.out - y_rate_pid.out - h_pid.out);
Motor_Set4(throttle + p_rate_pid.out + r_rate_pid.out + y_rate_pid.out - h_pid.out);
}
else
Motor_Set(60, 60, 60, 60);
}
else
LED4(0);
if (PWM5_Time > 1700)
{
Motor_Set(0, 0, 0, 0);
p_rate_pid.iv = 0;
r_rate_pid.iv = 0;
y_rate_pid.iv = 0;
balence = 0;
pwmcon = 0;
}
if (dmp_retry > 200)
{
Motor_Set(0, 0, 0, 0);
balence = 0;
pwmcon = 0;
LED3(2);
break;
}
if (en_out_pwm)
{
rt_kprintf("\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",
PWM1_Time, PWM2_Time, PWM3_Time, PWM4_Time,
PWM5_Time, PWM6_Time, PWM7_Time, PWM8_Time);
// rt_kprintf("\t%d\t%d\t%d\t%d\t%d\t%d\n",
// Motor1,Motor2,Motor3,
// Motor4,Motor5,Motor6);
}
rt_thread_delay(2);
}
}
