Subversion Repositories FlightCtrl

Rev

Rev 1927 | Rev 2099 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1927 Rev 2024
Line 141... Line 141...
141 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
 141 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
142 
  uint8_t axis;
 142 
  uint8_t axis;
Line 143... Line 143...
143 
 
 143 
 
144 
    // TODO: Multiply on a factor on control. Wont work without...
 144 
    // TODO: Multiply on a factor on control. Wont work without...
145 
  if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
 145 
  if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
146 
    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
 146 
    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.servoDirections & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
147 
    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
 147 
    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.servoDirections & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
Line 148... Line 148...
148 
    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
 148 
    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.servoDirections & 4 ? yawRate : -yawRate);
149 
 
 149 
 
150 
    angle[PITCH] += rate_ATT[PITCH];
 150 
    angle[PITCH] += rate_ATT[PITCH];
151 
    angle[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
 151 
    angle[ROLL]  += rate_ATT[ROLL];
152 
    angle[YAW] += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
 152 
    angle[YAW] += yawRate;
153 
} else {
 153 
} else {
154 
    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
 154 
    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
155 
    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
 155 
    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
156 
    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
 156 
    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? yawRate : -yawRate);
157 
    angle[PITCH] += rate_ATT[PITCH];
 157 
    angle[PITCH] += rate_ATT[PITCH];
158 
    angle[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
 158 
    angle[ROLL]  += rate_ATT[ROLL];
Line 159... Line 159...
159 
    angle[YAW] += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
 159 
    angle[YAW] += yawRate;
160 
  }
 160 
  }
161 
 
 161 
 
Line 205... Line 205...
205 
  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
 205 
  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
206 
  // are less than ....., or reintroduce Kalman.
 206 
  // are less than ....., or reintroduce Kalman.
207 
  // Well actually the Z axis acc. check is not so silly.
 207 
  // Well actually the Z axis acc. check is not so silly.
208 
  uint8_t axis;
 208 
  uint8_t axis;
209 
  int32_t temp;
 209 
  int32_t temp;
210 
  if (acc[Z] >= -staticParams.accCorrectionZAccLimit && acc[Z]
 210 
  if (acc[Z] >= -staticParams.zerothOrderGyroCorrectionZAccLimit && acc[Z]
211 
      <= dynamicParams.UserParams[7]) {
 211 
      <= dynamicParams.UserParams[7]) {
212 
    DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
 212 
    DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
Line 213... Line 213...
213 
 
 213 
 
214 
    uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
 214 
    uint8_t permilleAcc = staticParams.zerothOrderGyroCorrectionFactorx1000;
215 
    uint8_t debugFullWeight = 1;
 215 
    uint8_t debugFullWeight = 1;
Line 216... Line 216...
216 
    int32_t accDerived;
 216 
    int32_t accDerived;
217 
 
 217 
 
Line 282... Line 282...
282 
        round = DRIFTCORRECTION_TIME / 2;
 282 
        round = DRIFTCORRECTION_TIME / 2;
283 
      else
 283 
      else
284 
        round = -DRIFTCORRECTION_TIME / 2;
 284 
        round = -DRIFTCORRECTION_TIME / 2;
285 
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
 285 
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
286 
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
 286 
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
287 
      driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
 287 
      driftComp[axis] += deltaCorrection / staticParams.secondOrderGyroCorrectionDivisor;
288 
      CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
 288 
      CHECK_MIN_MAX(driftComp[axis], -staticParams.secondOrderGyroCorrectionLimit, staticParams.secondOrderGyroCorrectionLimit);
289 
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
 289 
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
290 
      DebugOut.Analog[16 + axis] = correctionSum[axis];
 290 
      DebugOut.Analog[16 + axis] = correctionSum[axis];
291 
      DebugOut.Analog[28 + axis] = driftComp[axis];
 291 
      DebugOut.Analog[28 + axis] = driftComp[axis];
Line 292... Line 292...
292 
 
 292