Subversion Repositories FlightCtrl

Rev

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

Rev 1910 Rev 1922
Line 38... Line 38...
38 
// With different (less) filtering
 38 
// With different (less) filtering
39 
int16_t rate_PID[2];
 39 
int16_t rate_PID[2];
40 
int16_t differential[3];
 40 
int16_t differential[3];
Line 41... Line 41...
41 
 
 41 
 
42 
/*
 - 
 
43 
 * Gyro readings, after performing "axis coupling" - that is, the transfomation
 - 
 
44 
 * of rotation rates from the airframe-local coordinate system to a ground-fixed
 - 
 
45 
 * coordinate system. If axis copling is disabled, the gyro readings will be
 - 
 
46 
 * copied into these directly.
 - 
 
47 
 * These are global for the same pragmatic reason as with the gyro readings.
 - 
 
48 
 * The variables are overwritten at each attitude calculation invocation - the values
 - 
 
49 
 * are not preserved or reused.
 - 
 
50 
 */
 - 
 
51 
int16_t ACRate[2], ACYawRate;
 - 
 
52 
 
 - 
 
53 
/*
 42 
/*
54 
 * Gyro integrals. These are the rotation angles of the airframe compared to the
 43 
 * Gyro integrals. These are the rotation angles of the airframe compared to the
- 
 
 44 
 * horizontal plane, yaw relative to yaw at start. Not really used for anything else
55 
 * horizontal plane, yaw relative to yaw at start.
 45 
 * than diagnostics.
56 
 */
 46 
 */
Line -... Line 47...
- 
 
 47 
int32_t angle[2], yawAngleDiff;
- 
 
 48 
 
- 
 
 49 
/*
- 
 
 50 
 * Error integrals. Stick is always positive. Gyro is configurable positive or negative.
57 
int32_t angle[2], yawAngleDiff;
 51 
 * These represent the deviation of the attitude angle from the desired on each axis.
Line 58... Line 52...
58 
 
 52 
 */
59 
int readingHeight = 0;
 - 
 
60 
 
 53 
int32_t error[3];
61 
// Yaw angle and compass stuff.
 54 
 
Line 62... Line 55...
62 
 
 55 
// Yaw angle and compass stuff.
63 
// This is updated/written from MM3. Negative angle indicates invalid data.
 56 
// This is updated/written from MM3. Negative angle indicates invalid data.
Line 162... Line 155...
162 
  // Interrupt-driven sensor reading may restart.
 155 
  // Interrupt-driven sensor reading may restart.
163 
  analogDataReady = 0;
 156 
  analogDataReady = 0;
164 
  analog_start();
 157 
  analog_start();
165 
}
 158 
}
Line 166... Line -...
166 
 
 - 
 
167 
/*
 - 
 
168 
 * This is the standard flight-style coordinate system transformation
 - 
 
169 
 * (from airframe-local axes to a ground-based system). For some reason
 - 
 
170 
 * the MK uses a left-hand coordinate system. The tranformation has been
 - 
 
171 
 * changed accordingly.
 - 
 
172 
 */
 - 
 
173 
void trigAxisCoupling(void) {
 - 
 
174 
  int16_t cospitch = int_cos(angle[PITCH]);
 - 
 
175 
  int16_t cosroll = int_cos(angle[ROLL]);
 - 
 
176 
  int16_t sinroll = int_sin(angle[ROLL]);
 - 
 
177 
 
 - 
 
178 
  ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate
 - 
 
179 
      * sinroll) >> MATH_UNIT_FACTOR_LOG);
 - 
 
180 
 
 - 
 
181 
  ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll
 - 
 
182 
      + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan(
 - 
 
183 
      angle[PITCH])) >> MATH_UNIT_FACTOR_LOG);
 - 
 
184 
 
 - 
 
185 
  ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
 - 
 
186 
 
 - 
 
187 
  ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
 - 
 
188 
}
 - 
 
189 
 
 - 
 
190 
// 480 usec with axis coupling - almost no time without.
 159 
 
191 
void integrate(void) {
 160 
void integrate(void) {
192 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
 161 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
Line 193... Line 162...
193 
  uint8_t axis;
 162 
  uint8_t axis;
- 
 
 163 
 
- 
 
 164 
  if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
194 
 
 165 
      error[PITCH] += (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
195 
  if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
 166 
      error[ROLL]  += (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
196 
    trigAxisCoupling();
 167 
      error[YAW]   += (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
197 
  } else {
 168 
  } else {
198 
    ACRate[PITCH] = rate_ATT[PITCH];
 169 
      error[PITCH] += (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
199 
    ACRate[ROLL] = rate_ATT[ROLL];
 170 
      error[ROLL]  += (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
Line -... Line 171...
- 
 
 171 
      error[YAW]   += (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
- 
 
 172 
  }
- 
 
 173 
 
- 
 
 174 
    for (axis=PITCH; axis<=YAW; axis++) {
- 
 
 175 
  if (error[axis] > ERRORLIMIT) {
- 
 
 176 
        error[axis] = ERRORLIMIT;
- 
 
 177 
    } else if (angle[axis] <= -ERRORLIMIT) {
- 
 
 178 
        angle[axis] = -ERRORLIMIT;
200 
    ACYawRate = yawRate;
 179 
    }
201 
  }
 180 
    }
202 
 
 181 
   
203 
  /*
 182 
  /*
204 
   * Yaw
 183 
   * Yaw
Line 216... Line 195...
216 
 
 195 
 
217 
  /*
 196 
  /*
218 
   * Pitch axis integration and range boundary wrap.
 197 
   * Pitch axis integration and range boundary wrap.
219 
   */
 198 
   */
220 
  for (axis = PITCH; axis <= ROLL; axis++) {
 199 
  for (axis = PITCH; axis <= ROLL; axis++) {
221 
    angle[axis] += ACRate[axis];
 200 
    angle[axis] += rate_ATT[axis];
222 
    if (angle[axis] > PITCHROLLOVER180) {
 201 
    if (angle[axis] > PITCHROLLOVER180) {
223 
      angle[axis] -= PITCHROLLOVER360;
 202 
      angle[axis] -= PITCHROLLOVER360;
224 
    } else if (angle[axis] <= -PITCHROLLOVER180) {
 203 
    } else if (angle[axis] <= -PITCHROLLOVER180) {
225 
      angle[axis] += PITCHROLLOVER360;
 204 
      angle[axis] += PITCHROLLOVER360;