Rev 886 | Go to most recent revision | Show entire file | Ignore 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; |