2020年10月20日 学习任务：完成Crazepony控制环的理解
之前是通过姿态解算获得了 四元数→旋转矩阵→欧拉角
-
CtrlAttiRate();
void CtrlAttiRate(void)
{
float yawRateTarget=0;
static uint32_t tPrev=0;
float dt=0,t=0;
t=micros();
dt=(tPrev>0)?(t-tPrev):0;
tPrev=t;//计算两次时间差
//定义了一个结构体yaw pitch roll throttle（油门）
yawRateTarget=-(float)RC_DATA.YAW;
//注意，原来的pid参数，对应的是 ad值,故转之
/*param 1是角速度PID中各个数据 param 2是角度PID中各个数据 param 3 没看懂是啥*/
#ifdef IMU_SW
PID_Postion_Cal(&pitch_rate_PID,pitch_angle_PID.Output,imu.gyro[PITCH]*180.0f/M_PI_F,dt);
PID_Postion_Cal(&roll_rate_PID,roll_angle_PID.Output,imu.gyro[ROLL]*180.0f/M_PI_F,dt);//gyroxGloble
PID_Postion_Cal(&yaw_rate_PID,yawRateTarget,imu.gyro[YAW]*180.0f/M_PI_F,dt);//DMP_DATA.GYROz
#else
//原参数对应于 DMP的直接输出gyro , 是deg. 且原DMP之后的处理运算是错误的
PID_Postion_Cal(&pitch_rate_PID,pitch_angle_PID.Output,imu.gyro[PITCH]*DMP_GYRO_SCALE,0);
PID_Postion_Cal(&roll_rate_PID,roll_angle_PID.Output,imu.gyro[ROLL]*DMP_GYRO_SCALE,0);//gyroxGloble
PID_Postion_Cal(&yaw_rate_PID,yawRateTarget,imu.gyro[YAW]*DMP_GYRO_SCALE,0); //DMP_DATA.GYROz
#endif
Pitch = pitch_rate_PID.Output;
Roll = roll_rate_PID.Output;
Yaw = yaw_rate_PID.Output;
}
1.2 采用位置式PID：
static void PID_Postion_Cal(PID_Typedef * PID,float target,float measure,int32_t dertT)
{
float termI=0;
float dt= dertT/1000000.0;
//误差=期望值-测量值
PID->Error=target-measure;
PID->Deriv= (PID->Error-PID->PreError)/dt;
PID->Output=(PID->P * PID->Error) + (PID->I * PID->Integ) + (PID->D * PID->Deriv); //PID:比例环节+积分环节+微分环节
PID->PreError=PID->Error;
//仅用于角度环和角速度环的
if(FLY_ENABLE && offLandFlag){
if(fabs(PID->Output) < Thro ) //比油门还大时不积分
{
termI=(PID->Integ) + (PID->Error) * dt; //积分环节
if(termI > - PID->iLimit && termI < PID->iLimit && PID->Output > - PID->iLimit && PID->Output < PID->iLimit) //在-300~300时才进行积分环节
PID->Integ=termI;
}
}else{
PID->Integ= 0;
}
}
CtrlMotor();
-
void CtrlMotor(void)
{
float cosTilt = imu.accb[2] / ONE_G;
if(altCtrlMode==MANUAL)
{
DIF_ACC.Z = imu.accb[2] - ONE_G;
Thro = RC_DATA.THROTTLE;
cosTilt=imu.DCMgb[2][2];
Thro=Thro/cosTilt;
}else{
Thro=(-thrustZSp) * 1000;// /imu.DCMgb[2][2]; //倾角补偿后效果不错，有时过猛
if(Thro>1000)
Thro=1000;
}
//将输出值融合到四个电机
Motor[2] = (int16_t)(Thro - Pitch - Roll - Yaw ); //M3
Motor[0] = (int16_t)(Thro + Pitch + Roll - Yaw ); //M1
Motor[3] = (int16_t)(Thro - Pitch + Roll + Yaw ); //M4
Motor[1] = (int16_t)(Thro + Pitch - Roll + Yaw ); //M2
if((FLY_ENABLE!=0))
MotorPwmFlash(Motor[0],Motor[1],Motor[2],Motor[3]);
else
MotorPwmFlash(0,0,0,0);
}
总结：
Crazepony飞机的总体框架看完 其他的细节算法打算采用minifly进行学习
还存在的疑惑有：
①没有见到使用 三轴角速度进行姿态解算
②电机控制中的 倾角补偿未看懂
02：36
October 20, 2020
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
