| 40,26 → 40,19 |
|---|
| int16_t differential[3]; |
| |
| /* |
| * Gyro readings, after performing "axis coupling" - that is, the transfomation |
| * of rotation rates from the airframe-local coordinate system to a ground-fixed |
| * coordinate system. If axis copling is disabled, the gyro readings will be |
| * copied into these directly. |
| * These are global for the same pragmatic reason as with the gyro readings. |
| * The variables are overwritten at each attitude calculation invocation - the values |
| * are not preserved or reused. |
| * Gyro integrals. These are the rotation angles of the airframe compared to the |
| * horizontal plane, yaw relative to yaw at start. Not really used for anything else |
| * than diagnostics. |
| */ |
| int16_t ACRate[2], ACYawRate; |
| int32_t angle[2], yawAngleDiff; |
| |
| /* |
| * Gyro integrals. These are the rotation angles of the airframe compared to the |
| * horizontal plane, yaw relative to yaw at start. |
| * Error integrals. Stick is always positive. Gyro is configurable positive or negative. |
| * These represent the deviation of the attitude angle from the desired on each axis. |
| */ |
| int32_t angle[2], yawAngleDiff; |
| int32_t error[3]; |
| |
| int readingHeight = 0; |
| |
| // Yaw angle and compass stuff. |
| |
| // This is updated/written from MM3. Negative angle indicates invalid data. |
| int16_t compassHeading = -1; |
| |
| 164,42 → 157,28 |
|---|
| analog_start(); |
| } |
| |
| /* |
| * This is the standard flight-style coordinate system transformation |
| * (from airframe-local axes to a ground-based system). For some reason |
| * the MK uses a left-hand coordinate system. The tranformation has been |
| * changed accordingly. |
| */ |
| void trigAxisCoupling(void) { |
| int16_t cospitch = int_cos(angle[PITCH]); |
| int16_t cosroll = int_cos(angle[ROLL]); |
| int16_t sinroll = int_sin(angle[ROLL]); |
| |
| ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate |
| * sinroll) >> MATH_UNIT_FACTOR_LOG); |
| |
| ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
| + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
| angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
| |
| ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
| |
| ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
| } |
| |
| // 480 usec with axis coupling - almost no time without. |
| void integrate(void) { |
| // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
| uint8_t axis; |
| |
| if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) { |
| trigAxisCoupling(); |
| error[PITCH] += (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
| error[ROLL] += (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| error[YAW] += (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| } else { |
| ACRate[PITCH] = rate_ATT[PITCH]; |
| ACRate[ROLL] = rate_ATT[ROLL]; |
| ACYawRate = yawRate; |
| error[PITCH] += (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
| error[ROLL] += (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| error[YAW] += (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| } |
| |
| for (axis=PITCH; axis<=YAW; axis++) { |
| if (error[axis] > ERRORLIMIT) { |
| error[axis] = ERRORLIMIT; |
| } else if (angle[axis] <= -ERRORLIMIT) { |
| angle[axis] = -ERRORLIMIT; |
| } |
| } |
| |
| /* |
| * Yaw |
| * Calculate yaw gyro integral (~ to rotation angle) |
| 218,7 → 197,7 |
|---|
| * Pitch axis integration and range boundary wrap. |
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| angle[axis] += ACRate[axis]; |
| angle[axis] += rate_ATT[axis]; |
| if (angle[axis] > PITCHROLLOVER180) { |
| angle[axis] -= PITCHROLLOVER360; |
| } else if (angle[axis] <= -PITCHROLLOVER180) { |