实验目的
1 了解和掌握控制理论与PID线性控制系统原理。
2 熟悉比例控制、积分控制、微分控制三种方法。
3 熟悉飞控算法PID框架设计、飞控算法外环PID实现、飞控算法内环PID实现方法。
4 了解信号滤波方法，包括移动平滑滤波、FIR滤波、IIR滤波等，知道卡尔曼滤波代码实现方法。
5 掌握直流电动机PWM对调节方法。
实验内容
1 控制理论与PID线性控制系统原理。
2 比例控制、积分控制、微分控制三种方法。
3 飞控算法PID框架设计、飞控算法外环PID实现、飞控算法内环PID实现。
4 信号滤波方法，包括移动平滑滤波、FIR滤波、IIR滤波等，卡尔曼滤波代码实现。
5 直流电动机PWM对调节。
void KalmanFilter(EKF_Filter_t *ekf, float input) //一维卡尔曼
{
ekf->Now_P = ekf->LastP + ekf->Q;
ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
ekf->LastP = (1 - ekf->Kg) * ekf->Now_P ;
}
void MotorControl(void)
{
volatile static uint8_t status = WAITING_1;
if(g_FMUflg.unlock == EMERGENT)
{
status = EXIT_255;
}
switch(status)
{
case WAITING_1:
if(g_FMUflg.unlock)
{
g_FMUflg.take_off = 0;
g_FMUflg.height_lock = 0;
status = WAITING_2;
}
case WAITING_2: //解锁完成后判断使用者是否开始拨动遥杆进行飞行控制
if(Remote.thr > 1100 && !g_FMUflg.take_off) //刚解锁时，如果不处于一键起飞并且遥杆处于最低，认为操作者还不想飞行
{
status = PROCESS_31;
}
else if(g_FMUflg.take_off)
{
g_FMUflg.height_lock = 1;
status = PROCESS_31;
}
else
{
break;
}
case PROCESS_31:
{
int16_t temp = 0;
//添加偏移紧急制动
if(g_Attitude.pitch < -MAX_ISFD_ATTITUDE
|| g_Attitude.pitch > MAX_ISFD_ATTITUDE
|| g_Attitude.roll < -MAX_ISFD_ATTITUDE
|| g_Attitude.roll > MAX_ISFD_ATTITUDE)
{
status = WAITING_1;
ResetAlt();
ResetPID();
ClearMotor;
ResetAltSpeed();
break;
}
if(g_UAVinfo.UAV_Mode == Stabilize_Mode)
{
temp = Remote.thr - 1000;
}
if(g_UAVinfo.UAV_Mode == Altitude_Hold)
{
temp = HeightInfo.Thr;
}
//将油门值作为基础值给PWM
MOTOR1 = LIMIT(temp, 0, MOTOR_MAX_INIT_VALUE);
MOTOR2 = LIMIT(temp, 0, MOTOR_MAX_INIT_VALUE);
MOTOR3 = LIMIT(temp, 0, MOTOR_MAX_INIT_VALUE);
MOTOR4 = LIMIT(temp, 0, MOTOR_MAX_INIT_VALUE);
MOTOR1 += +PIDGroup[emPID_Roll_Spd].out - PIDGroup[emPID_Pitch_Spd].out - PIDGroup[emPID_Yaw_Spd].out;
MOTOR2 += +PIDGroup[emPID_Roll_Spd].out + PIDGroup[emPID_Pitch_Spd].out + PIDGroup[emPID_Yaw_Spd].out;
MOTOR3 += -PIDGroup[emPID_Roll_Spd].out + PIDGroup[emPID_Pitch_Spd].out - PIDGroup[emPID_Yaw_Spd].out;
MOTOR4 += -PIDGroup[emPID_Roll_Spd].out - PIDGroup[emPID_Pitch_Spd].out + PIDGroup[emPID_Yaw_Spd].out;
}
break;
case EXIT_255:
status = WAITING_1; //返回等待解锁
ClearMotor;
break;
default:
break;
}
UpdateMotor(MOTOR4, MOTOR2, MOTOR1, MOTOR3);
}
实验方法步骤
1 学习控制理论与PID线性控制系统原理。
2 实现比例控制、积分控制、微分控制三种方法。
3 进行飞控算法PID框架设计、飞控算法外环PID实现、飞控算法内环PID实现。
4 进行信号滤波方法，包括移动平滑滤波、FIR滤波、IIR滤波等，卡尔曼滤波代码实现。
5 进行直流电动机PWM对调节。
