Rev 1910 | Rev 1927 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1910 | Rev 1922 | ||
|---|---|---|---|
| Line 137... | Line 137... | ||
| 137 | commands_handleCommands(); |
137 | commands_handleCommands(); |
| 138 | setNormalFlightParameters(); |
138 | setNormalFlightParameters(); |
| 139 | } |
139 | } |
| Line 140... | Line 140... | ||
| 140 | 140 | ||
| 141 | /************************************************************************/ |
- | |
| 142 | 141 | /************************************************************************/ |
|
| 143 | /* Calculate control feedback from angle (gyro integral) */ |
142 | /* Calculate control feedback from angle (gyro integral) */ |
| 144 | /* and angular velocity (gyro signal) */ |
143 | /* and angular velocity (gyro signal) */ |
| 145 | /************************************************************************/ |
144 | /************************************************************************/ |
| 146 | PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor / |
145 | PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor / |
| Line 152... | Line 151... | ||
| 152 | + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16; |
151 | + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16; |
| Line 153... | Line 152... | ||
| 153 | 152 | ||
| 154 | PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING) |
153 | PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING) |
| Line 155... | Line -... | ||
| 155 | + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16; |
- | |
| - | 154 | + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16; |
|
| - | 155 | ||
| - | 156 | /************************************************************************/ |
|
| 156 | 157 | /* Stick signals are positive and gyros are negative... */ |
|
| 157 | 158 | /************************************************************************/ |
|
| 158 | IPart[PITCH] = controlIntegrals[CONTROL_ELEVATOR] - angle[PITCH]; |
159 | IPart[PITCH] = controlIntegrals[CONTROL_ELEVATOR] - angle[PITCH]; |
| 159 | if (IPart[PITCH] > PITCHROLLOVER180) IPart[PITCH] -= PITCHROLLOVER360; |
160 | if (IPart[PITCH] > PITCHROLLOVER180) IPart[PITCH] -= PITCHROLLOVER360; |
| 160 | else if (IPart[PITCH] <= -PITCHROLLOVER180) IPart[PITCH] += PITCHROLLOVER360; |
161 | else if (IPart[PITCH] <= -PITCHROLLOVER180) IPart[PITCH] += PITCHROLLOVER360; |