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