Rev 2057 | Rev 2059 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2057 | Rev 2058 | ||
---|---|---|---|
Line 67... | Line 67... | ||
67 | IPart[PITCH] = IPart[ROLL] = 0; |
67 | IPart[PITCH] = IPart[ROLL] = 0; |
68 | headingError = 0; |
68 | headingError = 0; |
69 | } |
69 | } |
Line 70... | Line 70... | ||
70 | 70 | ||
- | 71 | void flight_takeOff() { |
|
- | 72 | // This is for GPS module to mark home position. |
|
- | 73 | // TODO: What a disgrace, change it. |
|
- | 74 | MKFlags |= MKFLAG_CALIBRATE; |
|
71 | void flight_takeOff() { |
75 | |
72 | HC_setGround(); |
76 | HC_setGround(); |
73 | #ifdef USE_MK3MAG |
77 | #ifdef USE_MK3MAG |
74 | attitude_resetHeadingToMagnetic(); |
78 | attitude_resetHeadingToMagnetic(); |
75 | compass_setTakeoffHeading(heading); |
79 | compass_setTakeoffHeading(heading); |
Line 117... | Line 121... | ||
117 | else if (throttleTerm > staticParams.maxThrottle - 20) |
121 | else if (throttleTerm > staticParams.maxThrottle - 20) |
118 | throttleTerm = (staticParams.maxThrottle - 20); |
122 | throttleTerm = (staticParams.maxThrottle - 20); |
Line 119... | Line 123... | ||
119 | 123 | ||
120 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
124 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
121 | throttleTerm *= CONTROL_SCALING; |
- | |
Line 122... | Line 125... | ||
122 | // TODO: We dont need to repeat this for every iteration! |
125 | throttleTerm *= CONTROL_SCALING; |
123 | 126 | ||
124 | // end part 1: 750-800 usec. |
127 | // end part 1: 750-800 usec. |
125 | // start part 3: 350 - 400 usec. |
128 | // start part 3: 350 - 400 usec. |
126 | #define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
129 | #define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
- | 130 | // This is where control affects the target heading. It also (later) directly controls yaw. |
|
127 | // This is where control affects the target heading. It also (later) directly controls yaw. |
131 | headingError -= controls[CONTROL_YAW]; |
128 | headingError -= controls[CONTROL_YAW]; |
132 | |
129 | if (headingError < -YAW_I_LIMIT) |
133 | if (headingError < -YAW_I_LIMIT) |
130 | headingError = -YAW_I_LIMIT; |
134 | headingError = -YAW_I_LIMIT; |
Line -... | Line 135... | ||
- | 135 | else if (headingError > YAW_I_LIMIT) |
|
- | 136 | headingError = YAW_I_LIMIT; |
|
131 | if (headingError > YAW_I_LIMIT) |
137 | |
132 | headingError = YAW_I_LIMIT; |
138 | debugOut.analog[29] = headingError / 100; |
133 | 139 | ||
Line 134... | Line 140... | ||
134 | PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4); |
140 | PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4); |
Line 187... | Line 193... | ||
187 | /* and angular velocity (gyro signal) */ |
193 | /* and angular velocity (gyro signal) */ |
188 | /************************************************************************/ |
194 | /************************************************************************/ |
189 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
195 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
190 | for (axis = PITCH; axis <= ROLL; axis++) { |
196 | for (axis = PITCH; axis <= ROLL; axis++) { |
191 | PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4); |
197 | PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4); |
192 | PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
- | |
193 | // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick. |
198 | // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick. |
194 | // In HH mode, the I part is summed from P and D of gyros minus stick. |
199 | // In HH mode, the I part is summed from P and D of gyros minus stick. |
195 | if (gyroIFactor) { |
200 | if (gyroIFactor) { |
196 | int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
201 | int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); |
- | 202 | if (axis == 0) debugOut.analog[28] = iDiff; |
|
197 | PDPart += iDiff; |
203 | PDPart += iDiff; |
198 | IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
204 | IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
199 | } else { |
205 | } else { |
200 | IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
206 | IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
201 | } |
207 | } |
Line 207... | Line 213... | ||
207 | } else if (IPart[axis] > 64000) { |
213 | } else if (IPart[axis] > 64000) { |
208 | IPart[axis] = 64000; |
214 | IPart[axis] = 64000; |
209 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
215 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
210 | } |
216 | } |
Line -... | Line 217... | ||
- | 217 | ||
- | 218 | PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
|
211 | 219 | ||
212 | term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14); |
220 | term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14); |
Line 213... | Line 221... | ||
213 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
221 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
214 | 222 | ||
Line 245... | Line 253... | ||
245 | } |
253 | } |
Line 246... | Line 254... | ||
246 | 254 | ||
247 | debugOut.analog[8] = yawTerm; |
255 | debugOut.analog[8] = yawTerm; |
Line 248... | Line 256... | ||
248 | debugOut.analog[9] = throttleTerm; |
256 | debugOut.analog[9] = throttleTerm; |
Line 249... | Line 257... | ||
249 | 257 | ||
250 | debugOut.analog[16] = gyroActivity; |
258 | //debugOut.analog[16] = gyroActivity; |
251 | 259 |