44,7 → 44,7 |
* horizontal plane, yaw relative to yaw at start. Not really used for anything else |
* than diagnostics. |
*/ |
int32_t angle[2], yawAngleDiff; |
int32_t angle[2]; |
|
/* |
* Error integrals. Stick is always positive. Gyro is configurable positive or negative. |
68,7 → 68,6 |
uint16_t ignoreCompassTimer = 500; |
|
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t yawGyroDrift; |
|
int16_t correctionSum[2] = { 0, 0 }; |
|
111,7 → 110,7 |
************************************************************************/ |
void attitude_setNeutral(void) { |
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
driftComp[PITCH] = driftComp[ROLL]; |
correctionSum[PITCH] = correctionSum[ROLL] = 0; |
|
// Calibrate hardware. |
119,7 → 118,6 |
|
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
yawAngleDiff = 0; |
|
// update compass course to current heading |
compassCourse = compassHeading; |
161,18 → 159,19 |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
|
// TODO: Multiply on a factor. Wont work without... |
error[PITCH] += control[CONTROL_ELEVATOR]; |
error[ROLL] += control[CONTROL_AILERONS]; |
error[YAW] += control[CONTROL_RUDDER]; |
|
if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) { |
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); |
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
} else { |
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); |
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
} |
|
for (axis=PITCH; axis<=YAW; axis++) { |
189,7 → 188,6 |
* Limit yawGyroHeading proportional to 0 deg to 360 deg |
*/ |
yawGyroHeading += ACYawRate; |
yawAngleDiff += yawRate; |
|
if (yawGyroHeading >= YAWOVER360) { |
yawGyroHeading -= YAWOVER360; // 360 deg. wrap |