//光流定点算法 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); }
//定高算法 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); } }
//自稳算法 void stable(float pitch, float roll, float yaw) { float yaw_err; PID_SetTarget(&p_angle_pid, pitch); 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_SetTarget(&r_angle_pid, roll); 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); yaw_err = ahrs.degree_yaw - yaw; PID_SetTarget(&y_angle_pid, 0); if (yaw_err > 180.0f) yaw_err -= 360.0f; if (yaw_err < -180.0f) yaw_err += 360.0f; PID_xUpdate(&y_angle_pid, yaw_err); PID_SetTarget(&y_rate_pid, -RangeValue(y_angle_pid.out, -100, 100)); PID_xUpdate(&y_rate_pid, ahrs.gryo_yaw); }
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); } }