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