Subversion Repositories FlightCtrl

Rev

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

Rev 1926 Rev 1927
Line 42... Line 42...
42
/*
42
/*
43
 * 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
44
 * horizontal plane, yaw relative to yaw at start. Not really used for anything else
45
 * than diagnostics.
45
 * than diagnostics.
46
 */
46
 */
47
int32_t angle[2];
47
int32_t angle[3];
Line 48... Line 48...
48
 
48
 
49
/*
49
/*
50
 * Error integrals. Stick is always positive. Gyro is configurable positive or negative.
50
 * Error integrals. Stick is always positive. Gyro is configurable positive or negative.
51
 * These represent the deviation of the attitude angle from the desired on each axis.
51
 * These represent the deviation of the attitude angle from the desired on each axis.
52
 */
52
 */
Line 53... Line -...
53
int32_t error[3];
-
 
54
 
-
 
55
// Yaw angle and compass stuff.
-
 
56
// This is updated/written from MM3. Negative angle indicates invalid data.
-
 
57
int16_t compassHeading = -1;
-
 
58
 
-
 
59
// This is NOT updated from MM3. Negative angle indicates invalid data.
-
 
60
int16_t compassCourse = -1;
-
 
61
 
-
 
62
// The difference between the above 2 (heading - course) on a -180..179 degree interval.
-
 
63
// Not necessary. Never read anywhere.
-
 
64
// int16_t compassOffCourse = 0;
-
 
65
 
-
 
66
uint8_t updateCompassCourse = 0;
-
 
67
uint8_t compassCalState = 0;
-
 
68
uint16_t ignoreCompassTimer = 500;
53
int32_t error[3];
Line 69... Line 54...
69
 
54
 
Line 70... Line 55...
70
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
55
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
Line 117... Line 102...
117
  analog_calibrate();
102
  analog_calibrate();
Line 118... Line 103...
118
 
103
 
119
  // reset gyro integrals to acc guessing
104
  // reset gyro integrals to acc guessing
Line 120... Line -...
120
  setStaticAttitudeAngles();
-
 
121
 
-
 
122
  // update compass course to current heading
-
 
123
  compassCourse = compassHeading;
105
  setStaticAttitudeAngles();
124
 
106
 
Line 125... Line 107...
125
  // Inititialize YawGyroIntegral value with current compass heading
107
  // Inititialize YawGyroIntegral value with current compass heading
126
  yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW;
108
  angle[YAW] = 0;
Line 127... Line 109...
127
 
109
 
Line 157... Line 139...
157
 
139
 
158
void integrate(void) {
140
void integrate(void) {
159
  // 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.
Line 160... Line 142...
160
  uint8_t axis;
142
  uint8_t axis;
161
 
-
 
162
    // TODO: Multiply on a factor. Wont work without...
-
 
163
  error[PITCH] += control[CONTROL_ELEVATOR];
-
 
164
  error[ROLL] += control[CONTROL_AILERONS];
-
 
165
  error[YAW] += control[CONTROL_RUDDER];
143
 
166
   
144
    // TODO: Multiply on a factor on control. Wont work without...
167
  if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
145
  if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
168
      error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
146
    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
-
 
147
    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
-
 
148
    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
-
 
149
 
-
 
150
    angle[PITCH] += rate_ATT[PITCH];
169
      error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
151
    angle[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
170
      error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
152
    angle[YAW] += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
171
  } else {
153
} else {
172
      error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
154
    error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
-
 
155
    error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
-
 
156
    error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
-
 
157
    angle[PITCH] += rate_ATT[PITCH];
173
      error[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
158
    angle[ROLL]  += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL]  : -rate_ATT[ROLL]);
Line -... Line 159...
-
 
159
    angle[YAW] += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
-
 
160
  }
174
      error[YAW]   += control[CONTROL_RUDDER]   + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
161
 
175
  }
162
// TODO: Configurable.
176
 
163
#define ERRORLIMIT 1000
177
    for (axis=PITCH; axis<=YAW; axis++) {
164
for (axis=PITCH; axis<=YAW; axis++) {
178
  if (error[axis] > ERRORLIMIT) {
165
  if (error[axis] > ERRORLIMIT) {
179
        error[axis] = ERRORLIMIT;
166
        error[axis] = ERRORLIMIT;
180
    } else if (angle[axis] <= -ERRORLIMIT) {
167
    } else if (angle[axis] <= -ERRORLIMIT) {
Line 181... Line 168...
181
        angle[axis] = -ERRORLIMIT;
168
        angle[axis] = -ERRORLIMIT;
182
    }
-
 
183
    }
-
 
184
   
-
 
185
  /*
-
 
186
   * Yaw
-
 
187
   * Calculate yaw gyro integral (~ to rotation angle)
-
 
188
   * Limit yawGyroHeading proportional to 0 deg to 360 deg
-
 
189
   */
-
 
190
  yawGyroHeading += ACYawRate;
-
 
191
 
-
 
192
  if (yawGyroHeading >= YAWOVER360) {
-
 
193
    yawGyroHeading -= YAWOVER360; // 360 deg. wrap
-
 
194
  } else if (yawGyroHeading < 0) {
-
 
195
    yawGyroHeading += YAWOVER360;
169
    }
196
  }
170
}
197
 
171
   
198
  /*
172
  /*
199
   * Pitch axis integration and range boundary wrap.
173
   * Pitch axis integration and range boundary wrap.
200
   */
174
   */
201
  for (axis = PITCH; axis <= ROLL; axis++) {
175
  for (axis = PITCH; axis <= ROLL; axis++) {
202
    angle[axis] += rate_ATT[axis];
176
    angle[axis] += rate_ATT[axis];
203
    if (angle[axis] > PITCHROLLOVER180) {
177
    if (angle[axis] > PITCHROLLOVER180) {
204
      angle[axis] -= PITCHROLLOVER360;
178
      angle[axis] -= PITCHROLLOVER360;
-
 
179
    } else if (angle[axis] <= -PITCHROLLOVER180) {
-
 
180
      angle[axis] += PITCHROLLOVER360;
-
 
181
    }
-
 
182
  }
-
 
183
 
-
 
184
    /*
-
 
185
     * Yaw
-
 
186
     * Calculate yaw gyro integral (~ to rotation angle)
-
 
187
     * Limit yawGyroHeading proportional to 0 deg to 360 deg
-
 
188
     */
-
 
189
    if (angle[YAW] >= YAWOVER360) {
-
 
190
        angle[YAW] -= YAWOVER360; // 360 deg. wrap
205
    } else if (angle[axis] <= -PITCHROLLOVER180) {
191
    } else if (angle[YAW] < 0) {
Line 206... Line 192...
206
      angle[axis] += PITCHROLLOVER360;
192
        angle[YAW] += YAWOVER360;
207
    }
193
    }
208
  }
194
   
Line 219... Line 205...
219
  // 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
220
  // are less than ....., or reintroduce Kalman.
206
  // are less than ....., or reintroduce Kalman.
221
  // Well actually the Z axis acc. check is not so silly.
207
  // Well actually the Z axis acc. check is not so silly.
222
  uint8_t axis;
208
  uint8_t axis;
223
  int32_t temp;
209
  int32_t temp;
224
  if (acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
210
  if (acc[Z] >= -staticParams.accCorrectionZAccLimit && acc[Z]
225
      <= dynamicParams.UserParams[7]) {
211
      <= dynamicParams.UserParams[7]) {
226
    DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
212
    DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
Line 227... Line 213...
227
 
213
 
228
    uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
214
    uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!