| 143,20 → 143,20 |
|---|
| |
| // TODO: Multiply on a factor on control. Wont work without... |
| if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) { |
| 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); |
| error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.servoDirections & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
| error[ROLL] += control[CONTROL_AILERONS] + (staticParams.servoDirections & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| error[YAW] += control[CONTROL_RUDDER] + (staticParams.servoDirections & 4 ? yawRate : -yawRate); |
| |
| angle[PITCH] += rate_ATT[PITCH]; |
| angle[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| angle[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| angle[ROLL] += rate_ATT[ROLL]; |
| angle[YAW] += yawRate; |
| } else { |
| 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); |
| error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
| error[ROLL] += control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| error[YAW] += control[CONTROL_RUDDER] + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? yawRate : -yawRate); |
| angle[PITCH] += rate_ATT[PITCH]; |
| angle[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
| angle[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate); |
| angle[ROLL] += rate_ATT[ROLL]; |
| angle[YAW] += yawRate; |
| } |
| |
| // TODO: Configurable. |
| 207,11 → 207,11 |
|---|
| // Well actually the Z axis acc. check is not so silly. |
| uint8_t axis; |
| int32_t temp; |
| if (acc[Z] >= -staticParams.accCorrectionZAccLimit && acc[Z] |
| if (acc[Z] >= -staticParams.zerothOrderGyroCorrectionZAccLimit && acc[Z] |
| <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| |
| uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
| uint8_t permilleAcc = staticParams.zerothOrderGyroCorrectionFactorx1000; |
| uint8_t debugFullWeight = 1; |
| int32_t accDerived; |
| |
| 284,8 → 284,8 |
|---|
| round = -DRIFTCORRECTION_TIME / 2; |
| deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
| // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| driftComp[axis] += deltaCorrection / staticParams.secondOrderGyroCorrectionDivisor; |
| CHECK_MIN_MAX(driftComp[axis], -staticParams.secondOrderGyroCorrectionLimit, staticParams.secondOrderGyroCorrectionLimit); |
| // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| DebugOut.Analog[16 + axis] = correctionSum[axis]; |
| DebugOut.Analog[28 + axis] = driftComp[axis]; |