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