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) { |