MK四轴飞控里面的若干问题，请教！谢谢
本帖最后由 小笨蛋 于 2012-9-14 08:21 编辑想做四轴，硬件已经搭好，现在研究飞控了，MK的飞控好复杂，看了好多遍还有好多地方不懂，前来请教一下。 // Coupling fraction
if(!Looping_Nick && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
{
tmpl = (Reading_GyroYaw * Reading_IntegralGyroNick) / 2048L;
tmpl *= FCParam.Yaw_PosFeedback;
tmpl /= 4096L;
tmpl2 = ( Reading_GyroYaw * Reading_IntegralGyroRoll) / 2048L;
tmpl2 *= FCParam.Yaw_PosFeedback;
tmpl2 /= 4096L;
if(labs(tmpl) > 128 || labs(tmpl2) > 128) FunnelCourse = 1;
}
elsetmpl = tmpl2 = 0;这个地方怎么把Reading_GyroYaw 和Reading_IntegralGyroNick两个轴乘到一起？这有什么意义？ int32_t tmp_long, tmp_long2;
if(FCParam.Kalman_K != -1)
{
// determine the deviation of gyro integral from averaged acceleration sensor
tmp_long = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
tmp_long = (tmp_long * FCParam.Kalman_K) / (32 * 16);
tmp_long2= (int32_t)(IntegralRoll / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
tmp_long2= (tmp_long2 * FCParam.Kalman_K) / (32 * 16);
if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
{
tmp_long/= 2;
tmp_long2 /= 2;
}
if(abs(PPM_in]) > 25) // reduce further if yaw stick is active
{
tmp_long/= 3;
tmp_long2 /= 3;
}
// limit correction effect
if(tmp_long >(int32_t)FCParam.Kalman_MaxFusion)tmp_long= (int32_t)FCParam.Kalman_MaxFusion;
if(tmp_long < -(int32_t)FCParam.Kalman_MaxFusion)tmp_long=-(int32_t)FCParam.Kalman_MaxFusion;
if(tmp_long2 > (int32_t)FCParam.Kalman_MaxFusion)tmp_long2 = (int32_t)FCParam.Kalman_MaxFusion;
if(tmp_long2 <-(int32_t)FCParam.Kalman_MaxFusion)tmp_long2 =-(int32_t)FCParam.Kalman_MaxFusion;
}这个地方 (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);陀螺仪和加速度计数据可以直接比较吗？难道加速度计不需要用反正切得出角度？ if(!Looping_Nick && !Looping_Roll && !FunnelCourse)
{
// Calculate mean value of the gyro integrals
MeanIntegralNick /= BALANCE_NUMBER;
MeanIntegralRoll/= BALANCE_NUMBER;
// Calculate mean of the acceleration values
IntegralAccNick = (ParamSet.GyroAccFactor * IntegralAccNick) / BALANCE_NUMBER;
IntegralAccRoll= (ParamSet.GyroAccFactor * IntegralAccRoll ) / BALANCE_NUMBER;
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorNick = (int32_t)(MeanIntegralNick - (int32_t)IntegralAccNick);
CorrectionNick = IntegralErrorNick / ParamSet.GyroAccTrim;
AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
CorrectionRoll= IntegralErrorRoll / ParamSet.GyroAccTrim;
AttitudeCorrectionRoll= CorrectionRoll/ BALANCE_NUMBER;
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in]) > 25)) && (FCParam.Kalman_K == -1) )
{
AttitudeCorrectionNick /= 2;
AttitudeCorrectionRoll /= 2;
}
上面不是已经 MeanIntegralNick /= BALANCE_NUMBER; 了吗，为什么还要再除一次？ AttitudeCorrectionNick = CorrectionNick / BALANCE_NUMBER;// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Nick-Axis
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DiffNick = Reading_GyroNick - StickNick; // get difference
if(Gyro_I_Factor) SumNick += IntegralNick * Gyro_I_Factor - StickNick; // I-part for attitude control
else SumNick += DiffNick; // I-part for head holding
if(SumNick >(STICK_GAIN * 16000L)) SumNick =(STICK_GAIN * 16000L);
if(SumNick < -(STICK_GAIN * 16000L)) SumNick = -(STICK_GAIN * 16000L);
pd_result = DiffNick + Ki * SumNick; // PI-controller for nick
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64;
if(pd_result >tmp_int) pd_result =tmp_int;
if(pd_result < -tmp_int) pd_result = -tmp_int;
// Motor Front
MotorValue = GasMixFraction + pd_result + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Front = MotorValue;
// Motor Rear
MotorValue = GasMixFraction - pd_result + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Rear = MotorValue;最后这个
DiffNick = Reading_GyroNick - StickNick; // get difference
if(Gyro_I_Factor) SumNick += IntegralNick * Gyro_I_Factor - StickNick; // I-part for attitude control
else SumNick += DiffNick; // I-part for head holding
pd_result = DiffNick + Ki * SumNick; // PI-controller for nick
地方不懂。。
Reading_GyroNick应该是角速度，为什么能和StickNick相减呢？
后面IntegralNick 已经是积分了的，为什么 SumNick +=还要积分一次？
最后的电机控制量应该是：角速度*a+角度*b 吧？
还是 角速度*a+角度*b+∫角度*c？？
恳请高手指教一下，谢谢了 直流电机，掌上四轴，PWM直接控制
页:
[1]