Rev 1868 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1868 | Rev 1869 | ||
|---|---|---|---|
| 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 |
|
| 207 | rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
208 | + driftComp[axis]; |
| - | 209 | rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR |
|
| 208 | rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
210 | + driftComp[axis]; |
| 209 | differential[axis] = gyroD[axis]; |
211 | differential[axis] = gyroD[axis]; |
| 210 | averageAcc[axis] += acc[axis]; |
212 | averageAcc[axis] += acc[axis]; |
| Line 211... | Line 213... | ||
| 211 | } |
213 | } |
| Line 224... | Line 226... | ||
| 224 | * (from airframe-local axes to a ground-based system). For some reason |
226 | * (from airframe-local axes to a ground-based system). For some reason |
| 225 | * the MK uses a left-hand coordinate system. The tranformation has been |
227 | * the MK uses a left-hand coordinate system. The tranformation has been |
| 226 | * changed accordingly. |
228 | * changed accordingly. |
| 227 | */ |
229 | */ |
| 228 | void trigAxisCoupling(void) { |
230 | void trigAxisCoupling(void) { |
| - | 231 | J5HIGH; |
|
| 229 | int16_t cospitch = int_cos(angle[PITCH]); |
232 | int16_t cospitch = int_cos(angle[PITCH]); |
| 230 | int16_t cosroll = int_cos(angle[ROLL]); |
233 | int16_t cosroll = int_cos(angle[ROLL]); |
| 231 | int16_t sinroll = int_sin(angle[ROLL]); |
234 | int16_t sinroll = int_sin(angle[ROLL]); |
| Line 232... | Line 235... | ||
| 232 | 235 | ||
| 233 | ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
236 | ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
| Line 234... | Line -... | ||
| 234 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
- | |
| 235 | 237 | * sinroll) >> MATH_UNIT_FACTOR_LOG); |
|
| 236 | ACRate[ROLL] = rate_ATT[ROLL] + |
238 | |
| 237 | (((((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) |
239 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t) rate_ATT[PITCH] * sinroll |
| Line 238... | Line 240... | ||
| 238 | >> MATH_UNIT_FACTOR_LOG) |
240 | + (int32_t) yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
| 239 | * int_tan(angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
241 | angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
| 240 | 242 | ||
| Line 278... | Line 280... | ||
| 278 | angle[axis] -= PITCHROLLOVER360; |
280 | angle[axis] -= PITCHROLLOVER360; |
| 279 | } else if (angle[axis] <= -PITCHROLLOVER180) { |
281 | } else if (angle[axis] <= -PITCHROLLOVER180) { |
| 280 | angle[axis] += PITCHROLLOVER360; |
282 | angle[axis] += PITCHROLLOVER360; |
| 281 | } |
283 | } |
| 282 | } |
284 | } |
| - | 285 | J5LOW; |
|
| 283 | } |
286 | } |
| Line 284... | Line 287... | ||
| 284 | 287 | ||
| 285 | /************************************************************************ |
288 | /************************************************************************ |
| 286 | * A kind of 0'th order integral correction, that corrects the integrals |
289 | * A kind of 0'th order integral correction, that corrects the integrals |
| Line 362... | Line 365... | ||
| 362 | uint8_t axis; |
365 | uint8_t axis; |
| 363 | if (!--timer) { |
366 | if (!--timer) { |
| 364 | timer = DRIFTCORRECTION_TIME; |
367 | timer = DRIFTCORRECTION_TIME; |
| 365 | for (axis = PITCH; axis <= ROLL; axis++) { |
368 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 366 | // Take the sum of corrections applied, add it to delta |
369 | // Take the sum of corrections applied, add it to delta |
| 367 | deltaCorrection = (correctionSum[axis] + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
370 | deltaCorrection = (correctionSum[axis] + DRIFTCORRECTION_TIME / 2) |
| - | 371 | / DRIFTCORRECTION_TIME; |
|
| 368 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
372 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| 369 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
373 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| 370 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
374 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| 371 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
375 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| 372 | DebugOut.Analog[16 + axis] = correctionSum[axis]; |
376 | DebugOut.Analog[16 + axis] = correctionSum[axis]; |
| Line 388... | Line 392... | ||
| 388 | getAnalogData(); |
392 | getAnalogData(); |
| 389 | // end part1b |
393 | // end part1b |
| 390 | integrate(); |
394 | integrate(); |
| 391 | // end part1a |
395 | // end part1a |
| Line 392... | Line -... | ||
| 392 | - | ||
| 393 | 396 | ||
| 394 | DebugOut.Analog[6] = ACRate[PITCH]; |
397 | DebugOut.Analog[6] = stronglyFilteredAcc[PITCH]; |
| 395 | DebugOut.Analog[7] = ACRate[ROLL]; |
398 | DebugOut.Analog[7] = stronglyFilteredAcc[ROLL]; |
| Line 396... | Line 399... | ||
| 396 | DebugOut.Analog[8] = ACYawRate; |
399 | DebugOut.Analog[8] = stronglyFilteredAcc[Z]; |
| 397 | 400 | ||
| 398 | DebugOut.Analog[3] = rate_PID[PITCH]; |
401 | DebugOut.Analog[3] = rate_PID[PITCH]; |