//更新电机
void motor_update(u16 th)
{
Motor_Set1(th - p_rate_pid.out - r_rate_pid.out + y_rate_pid.out);
Motor_Set2(th - p_rate_pid.out + r_rate_pid.out - y_rate_pid.out);
Motor_Set3(th + p_rate_pid.out - r_rate_pid.out - y_rate_pid.out);
Motor_Set4(th + p_rate_pid.out + r_rate_pid.out + y_rate_pid.out);
}
//带定高更新电机
void motor_hupdate(u16 th)
{
float weight;
weight = th / 400.0f;
weight = RangeValue(weight, 0, 1);
Motor_Set1(th - p_rate_pid.out - r_rate_pid.out + y_rate_pid.out - weight * h_v_pid.out);
Motor_Set2(th - p_rate_pid.out + r_rate_pid.out - y_rate_pid.out - weight * h_v_pid.out);
Motor_Set3(th + p_rate_pid.out - r_rate_pid.out - y_rate_pid.out - weight * h_v_pid.out);
Motor_Set4(th + p_rate_pid.out + r_rate_pid.out + y_rate_pid.out - weight * h_v_pid.out);
}
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);
}
}
