Rev 1869 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1869 | Rev 1870 | ||
|---|---|---|---|
| Line 202... | Line 202... | ||
| 202 | *************************************************************************/ |
202 | *************************************************************************/ |
| 203 | void getAnalogData(void) { |
203 | void getAnalogData(void) { |
| 204 | uint8_t axis; |
204 | uint8_t axis; |
| Line 205... | Line 205... | ||
| 205 | 205 | ||
| 206 | for (axis = PITCH; axis <= ROLL; axis++) { |
206 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 207 | rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR |
- | |
| 208 | + driftComp[axis]; |
207 | rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| 209 | rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR |
- | |
| 210 | + driftComp[axis]; |
208 | rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| 211 | differential[axis] = gyroD[axis]; |
209 | differential[axis] = gyroD[axis]; |
| 212 | averageAcc[axis] += acc[axis]; |
210 | averageAcc[axis] += acc[axis]; |
| Line 213... | Line 211... | ||
| 213 | } |
211 | } |
| 214 | 212 | ||
| Line 215... | Line 213... | ||
| 215 | averageAccCount++; |
213 | averageAccCount++; |
| 216 | yawRate = yawGyro + driftCompYaw; |
214 | yawRate = yawGyro + driftCompYaw; |
| 217 | 215 | ||
| - | 216 | // We are done reading variables from the analog module. |
|
| 218 | // We are done reading variables from the analog module. |
217 | // Interrupt-driven sensor reading may restart. |
| 219 | // Interrupt-driven sensor reading may restart. |
218 | analogDataReady = 0; |
| Line 220... | Line 219... | ||
| 220 | analogDataReady = 0; |
219 | J4HIGH; |
| 221 | analog_start(); |
220 | analog_start(); |
| 222 | } |
221 | } |
| 223 | 222 | ||
| 224 | /* |
223 | /* |
| 225 | * This is the standard flight-style coordinate system transformation |
224 | * This is the standard flight-style coordinate system transformation |
| 226 | * (from airframe-local axes to a ground-based system). For some reason |
225 | * (from airframe-local axes to a ground-based system). For some reason |
| 227 | * the MK uses a left-hand coordinate system. The tranformation has been |
- | |
| 228 | * changed accordingly. |
226 | * the MK uses a left-hand coordinate system. The tranformation has been |
| 229 | */ |
227 | * changed accordingly. |
| 230 | void trigAxisCoupling(void) { |
228 | */ |
| Line 231... | Line 229... | ||
| 231 | J5HIGH; |
229 | void trigAxisCoupling(void) { |
| 232 | int16_t cospitch = int_cos(angle[PITCH]); |
230 | int16_t cospitch = int_cos(angle[PITCH]); |
| Line 233... | Line 231... | ||
| 233 | int16_t cosroll = int_cos(angle[ROLL]); |
231 | int16_t cosroll = int_cos(angle[ROLL]); |
| 234 | int16_t sinroll = int_sin(angle[ROLL]); |
232 | int16_t sinroll = int_sin(angle[ROLL]); |
| 235 | 233 | ||
| Line 236... | Line 234... | ||
| 236 | ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
234 | ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
| 237 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
- | |
| 238 | 235 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
|
| Line 239... | Line 236... | ||
| 239 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t) rate_ATT[PITCH] * sinroll |
236 | |
| 240 | + (int32_t) yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
237 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
| 241 | angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
238 | + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
| 242 | 239 | angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
|
| 243 | ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
240 | |
| 244 | + ((int32_t) yawRate * cosroll) / cospitch; |
- | |
| 245 | } |
241 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
| 246 | 242 | } |
|
| 247 | // 480 usec with axis coupling - almost no time without. |
243 | |
| 248 | void integrate(void) { |
244 | // 480 usec with axis coupling - almost no time without. |
| 249 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
245 | void integrate(void) { |
| Line 280... | Line 276... | ||
| 280 | angle[axis] -= PITCHROLLOVER360; |
276 | angle[axis] -= PITCHROLLOVER360; |
| 281 | } else if (angle[axis] <= -PITCHROLLOVER180) { |
277 | } else if (angle[axis] <= -PITCHROLLOVER180) { |
| 282 | angle[axis] += PITCHROLLOVER360; |
278 | angle[axis] += PITCHROLLOVER360; |
| 283 | } |
279 | } |
| 284 | } |
280 | } |
| 285 | J5LOW; |
- | |
| 286 | } |
281 | } |
| Line 287... | Line 282... | ||
| 287 | 282 | ||
| 288 | /************************************************************************ |
283 | /************************************************************************ |
| 289 | * A kind of 0'th order integral correction, that corrects the integrals |
284 | * A kind of 0'th order integral correction, that corrects the integrals |