Rev 886 | Rev 903 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 886 | Rev 901 | ||
---|---|---|---|
Line 1405... | Line 1405... | ||
1405 | // Roll-Axis |
1405 | // Roll-Axis |
1406 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1406 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1407 | DiffRoll = Reading_GyroRoll - StickRoll; // get difference |
1407 | DiffRoll = Reading_GyroRoll - StickRoll; // get difference |
1408 | if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - StickRoll; // I-part for attitude control |
1408 | if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - StickRoll; // I-part for attitude control |
1409 | else SumRoll += DiffRoll; // I-part for head holding |
1409 | else SumRoll += DiffRoll; // I-part for head holding |
1410 | if(SumRoll > 16000) SumRoll = 16000; |
1410 | if(SumRoll > (STICK_GAIN * 16000L)) SumRoll = (STICK_GAIN * 16000L); |
1411 | if(SumRoll < -16000) SumRoll = -16000; |
1411 | if(SumRoll < -(STICK_GAIN * 16000L)) SumRoll = -(STICK_GAIN * 16000L); |
1412 | pd_result = DiffRoll + Ki * SumRoll; // PI-controller for roll |
1412 | pd_result = DiffRoll + Ki * SumRoll; // PI-controller for roll |
1413 | tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
1413 | tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
1414 | if(pd_result > tmp_int) pd_result = tmp_int; |
1414 | if(pd_result > tmp_int) pd_result = tmp_int; |
1415 | if(pd_result < -tmp_int) pd_result = -tmp_int; |
1415 | if(pd_result < -tmp_int) pd_result = -tmp_int; |