Subversion Repositories FlightCtrl

Rev

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

Rev 2057 Rev 2058
Line 67... Line 67...
67 
  IPart[PITCH] = IPart[ROLL] = 0;
 67 
  IPart[PITCH] = IPart[ROLL] = 0;
68 
  headingError = 0;
 68 
  headingError = 0;
69 
}
 69 
}
Line 70... Line 70...
70 
 
 70 
 
- 
 
 71 
void flight_takeOff() {
- 
 
 72 
  // This is for GPS module to mark home position.
- 
 
 73 
  // TODO: What a disgrace, change it.
- 
 
 74 
  MKFlags |= MKFLAG_CALIBRATE;
71 
void flight_takeOff() {
 75 
 
72 
  HC_setGround();
 76 
  HC_setGround();
73 
#ifdef USE_MK3MAG
 77 
#ifdef USE_MK3MAG
74 
  attitude_resetHeadingToMagnetic();
 78 
  attitude_resetHeadingToMagnetic();
75 
  compass_setTakeoffHeading(heading);
 79 
  compass_setTakeoffHeading(heading);
Line 117... Line 121...
117 
  else if (throttleTerm > staticParams.maxThrottle - 20)
 121 
  else if (throttleTerm > staticParams.maxThrottle - 20)
118 
    throttleTerm = (staticParams.maxThrottle - 20);
 122 
    throttleTerm = (staticParams.maxThrottle - 20);
Line 119... Line 123...
119 
 
 123 
 
120 
  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
 124 
  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
121 
  throttleTerm *= CONTROL_SCALING;
 - 
 
Line 122... Line 125...
122 
  // TODO: We dont need to repeat this for every iteration!
 125 
  throttleTerm *= CONTROL_SCALING;
123 
 
 126 
 
124 
// end part 1: 750-800 usec.
 127 
// end part 1: 750-800 usec.
125 
// start part 3: 350 - 400 usec.
 128 
// start part 3: 350 - 400 usec.
126 
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
 129 
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
- 
 
 130 
// This is where control affects the target heading. It also (later) directly controls yaw.
127 
// This is where control affects the target heading. It also (later) directly controls yaw.
 131 
  headingError -= controls[CONTROL_YAW];
128 
  headingError -= controls[CONTROL_YAW];
 132 
 
129 
  if (headingError < -YAW_I_LIMIT)
 133 
  if (headingError < -YAW_I_LIMIT)
130 
    headingError = -YAW_I_LIMIT;
 134 
    headingError = -YAW_I_LIMIT;
Line -... Line 135...
- 
 
 135 
  else if (headingError > YAW_I_LIMIT)
- 
 
 136 
    headingError = YAW_I_LIMIT;
131 
  if (headingError > YAW_I_LIMIT)
 137 
 
132 
    headingError = YAW_I_LIMIT;
 138 
  debugOut.analog[29] = headingError / 100;
133 
 
 139 
 
Line 134... Line 140...
134 
  PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
 140 
  PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
Line 187... Line 193...
187 
  /* and angular velocity (gyro signal)                                   */
 193 
  /* and angular velocity (gyro signal)                                   */
188 
  /************************************************************************/
 194 
  /************************************************************************/
189 
  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
 195 
  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
190 
  for (axis = PITCH; axis <= ROLL; axis++) {
 196 
  for (axis = PITCH; axis <= ROLL; axis++) {
191 
    PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
 197 
    PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
192 
    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
 - 
 
193 
    // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
 198 
    // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
194 
    // In HH mode, the I part is summed from P and D of gyros minus stick.
 199 
    // In HH mode, the I part is summed from P and D of gyros minus stick.
195 
    if (gyroIFactor) {
 200 
    if (gyroIFactor) {
196 
      int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);
 201 
      int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
- 
 
 202 
      if (axis == 0) debugOut.analog[28] = iDiff;
197 
      PDPart += iDiff;
 203 
      PDPart += iDiff;
198 
      IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
 204 
      IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
199 
    } else {
 205 
    } else {
200 
      IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
 206 
      IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
201 
    }
 207 
    }
Line 207... Line 213...
207 
    } else if (IPart[axis] > 64000) {
 213 
    } else if (IPart[axis] > 64000) {
208 
      IPart[axis] = 64000;
 214 
      IPart[axis] = 64000;
209 
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
 215 
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
210 
    }
 216 
    }
Line -... Line 217...
- 
 
 217 
 
- 
 
 218 
    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
211 
 
 219 
 
212 
    term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14);
 220 
    term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14);
Line 213... Line 221...
213 
    term[axis] += (dynamicParams.levelCorrection[axis] - 128);
 221 
    term[axis] += (dynamicParams.levelCorrection[axis] - 128);
214 
 
 222 
 
Line 245... Line 253...
245 
  }
 253 
  }
Line 246... Line 254...
246 
 
 254 
 
247 
  debugOut.analog[8] = yawTerm;
 255 
  debugOut.analog[8] = yawTerm;
Line 248... Line 256...
248 
  debugOut.analog[9] = throttleTerm;
 256 
  debugOut.analog[9] = throttleTerm;
Line 249... Line 257...
249 
 
 257 
 
250 
  debugOut.analog[16] = gyroActivity;
 258 
  //debugOut.analog[16] = gyroActivity;
251 
 
 259