Subversion Repositories FlightCtrl

Rev

Rev 2047 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2047 Rev 2048
Line 1... Line -...
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
2
// + Copyright (c) 04.2007 Holger Buss
-
 
3
// + Nur f�r den privaten Gebrauch
-
 
4
// + www.MikroKopter.com
-
 
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
6
// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
-
 
7
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist.
-
 
8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
-
 
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
-
 
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
-
 
11
// + Verkauf von Luftbildaufnahmen, usw.
-
 
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
-
 
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
-
 
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
-
 
17
// + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
-
 
18
// + eindeutig als Ursprung verlinkt und genannt werden
-
 
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
20
// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
-
 
21
// + Benutzung auf eigene Gefahr
-
 
22
// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
-
 
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
-
 
25
// + mit unserer Zustimmung zul�ssig
-
 
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
-
 
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
-
 
30
// + this list of conditions and the following disclaimer.
-
 
31
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
-
 
32
// +     from this software without specific prior written permission.
-
 
33
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
-
 
34
// +     for non-commercial use (directly or indirectly)
-
 
35
// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
-
 
36
// +     with our written permission
-
 
37
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
-
 
38
// +     clearly linked as origin
-
 
39
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
-
 
40
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-
 
41
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-
 
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-
 
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-
 
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-
 
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-
 
46
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-
 
47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-
 
48
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-
 
49
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-
 
50
// +  POSSIBILITY OF SUCH DAMAGE.
-
 
51
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
52
/************************************************************************/
-
 
53
/* Flight Attitude                                                      */
-
 
54
/************************************************************************/
-
 
55
 
-
 
56
#include <stdlib.h>
1
#include <stdlib.h>
57
#include <avr/io.h>
2
#include <avr/io.h>
Line 58... Line 3...
58
 
3
 
59
#include "attitude.h"
4
#include "attitude.h"
-
 
5
#include "dongfangMath.h"
Line 60... Line 6...
60
#include "dongfangMath.h"
6
#include "commands.h"
61
 
7
 
Line 62... Line 8...
62
// For scope debugging only!
8
// For scope debugging only!
Line 100... Line 46...
100
 
46
 
101
/*
47
/*
102
 * Gyro integrals. These are the rotation angles of the airframe compared to the
48
 * Gyro integrals. These are the rotation angles of the airframe compared to the
103
 * horizontal plane, yaw relative to yaw at start.
49
 * horizontal plane, yaw relative to yaw at start.
104
 */
50
 */
Line 105... Line 51...
105
int32_t angle[2], yawAngleDiff;
51
int32_t attitude[2];
Line 106... Line 52...
106
 
52
 
Line 107... Line 53...
107
int readingHeight = 0;
53
//int readingHeight = 0;
108
 
54
 
Line 109... Line 55...
109
// Yaw angle and compass stuff.
55
// Yaw angle and compass stuff.
110
 
56
 
-
 
57
// This is updated/written from MM3. Negative angle indicates invalid data.
-
 
58
int16_t magneticHeading = -1;
Line 111... Line 59...
111
// This is updated/written from MM3. Negative angle indicates invalid data.
59
 
112
int16_t magneticHeading = -1;
60
// This is NOT updated from MM3. Negative angle indicates invalid data.
113
 
61
// int16_t headingInDegrees = -1;
Line 114... Line -...
114
// This is NOT updated from MM3. Negative angle indicates invalid data.
-
 
115
int16_t compassCourse = -1;
-
 
116
 
62
 
Line 117... Line 63...
117
// The difference between the above 2 (heading - course) on a -180..179 degree interval.
63
int32_t targetHeading;
118
// Not necessary. Never read anywhere.
64
 
Line 119... Line -...
119
// int16_t compassOffCourse = 0;
-
 
120
 
-
 
121
uint8_t updateCompassCourse = 0;
-
 
122
uint8_t compassCalState = 0;
-
 
123
uint16_t ignoreCompassTimer = 500;
65
// The difference between the above 2 (heading - course) on a -180..179 degree interval.
Line 124... Line 66...
124
 
66
// Not necessary. Never read anywhere.
125
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
67
// int16_t compassOffCourse = 0;
Line 154... Line 96...
154
 * it is hardly worth the trouble.                                      
96
 * it is hardly worth the trouble.                                      
155
 ************************************************************************/
97
 ************************************************************************/
Line 156... Line 98...
156
 
98
 
157
int32_t getAngleEstimateFromAcc(uint8_t axis) {
99
int32_t getAngleEstimateFromAcc(uint8_t axis) {
158
  //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L;
100
  //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L;
159
  return (int32_t)GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis];// + correctionTerm;
101
  return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm;
160
  // return 342L * filteredAcc[axis];
102
  // return 342L * filteredAcc[axis];
Line 161... Line 103...
161
}
103
}
162
 
104
 
163
void setStaticAttitudeAngles(void) {
105
void setStaticAttitudeAngles(void) {
164
#ifdef ATTITUDE_USE_ACC_SENSORS
106
#ifdef ATTITUDE_USE_ACC_SENSORS
165
  angle[PITCH] = getAngleEstimateFromAcc(PITCH);
107
  attitude[PITCH] = getAngleEstimateFromAcc(PITCH);
166
  angle[ROLL] = getAngleEstimateFromAcc(ROLL);
108
  attitude[ROLL] = getAngleEstimateFromAcc(ROLL);
167
#else
109
#else
168
  angle[PITCH] = angle[ROLL] = 0;
110
  attitude[PITCH] = attitude[ROLL] = 0;
Line 169... Line 111...
169
#endif
111
#endif
170
}
112
}
Line 182... Line 124...
182
  // Calibrate hardware.
124
  // Calibrate hardware.
183
  analog_setNeutral();
125
  analog_setNeutral();
Line 184... Line 126...
184
 
126
 
185
  // reset gyro integrals to acc guessing
127
  // reset gyro integrals to acc guessing
186
  setStaticAttitudeAngles();
-
 
187
  yawAngleDiff = 0;
-
 
188
 
-
 
189
  // update compass course to current heading
128
  setStaticAttitudeAngles();
190
  compassCourse = magneticHeading;
-
 
191
 
-
 
192
  // Inititialize YawGyroIntegral value with current compass heading
-
 
193
  yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
-
 
194
 
129
  attitude_resetHeadingToMagnetic();
195
  // Servo_On(); //enable servo output
130
  // Servo_On(); //enable servo output
Line 196... Line 131...
196
}
131
}
197
 
132
 
Line 222... Line 157...
222
 * (from airframe-local axes to a ground-based system). For some reason
157
 * (from airframe-local axes to a ground-based system). For some reason
223
 * the MK uses a left-hand coordinate system. The tranformation has been
158
 * the MK uses a left-hand coordinate system. The tranformation has been
224
 * changed accordingly.
159
 * changed accordingly.
225
 */
160
 */
226
void trigAxisCoupling(void) {
161
void trigAxisCoupling(void) {
227
  int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
162
  int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
228
  int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL;
163
  int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL;
Line 229... Line 164...
229
 
164
 
230
  int16_t cospitch = cos_360(pitchAngleInDegrees);
165
  int16_t cospitch = cos_360(pitchAngleInDegrees);
231
  int16_t cosroll = cos_360(rollAngleInDegrees);
166
  int16_t cosroll = cos_360(rollAngleInDegrees);
Line 232... Line 167...
232
  int16_t sinroll = sin_360(rollAngleInDegrees);
167
  int16_t sinroll = sin_360(rollAngleInDegrees);
233
 
168
 
234
  ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate
-
 
235
      * sinroll) >> LOG_MATH_UNIT_FACTOR);
-
 
236
 
-
 
Line -... Line 169...
-
 
169
  ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll
-
 
170
      - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR);
-
 
171
 
-
 
172
  ACRate[ROLL] = rate_ATT[ROLL]
-
 
173
      + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
-
 
174
          >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees))
237
  ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll
175
          >> LOG_MATH_UNIT_FACTOR);
-
 
176
 
238
      + (int32_t)yawRate * cosroll) >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) >> LOG_MATH_UNIT_FACTOR);
177
  ACYawRate =
-
 
178
      ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
239
 
179
          / cospitch;
-
 
180
 
240
  ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
181
  ACYawRate =
Line 241... Line 182...
241
 
182
      ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
242
  ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
183
          / cospitch;
243
}
184
}
Line 256... Line 197...
256
  }
197
  }
Line 257... Line 198...
257
 
198
 
258
  /*
199
  /*
259
   * Yaw
200
   * Yaw
260
   * Calculate yaw gyro integral (~ to rotation angle)
201
   * Calculate yaw gyro integral (~ to rotation angle)
261
   * Limit yawGyroHeading proportional to 0 deg to 360 deg
202
   * Limit heading proportional to 0 deg to 360 deg
262
   */
203
   */
263
  yawGyroHeading += ACYawRate;
-
 
264
  yawAngleDiff += yawRate;
-
 
265
 
204
  heading += ACYawRate;
266
  if (yawGyroHeading >= YAWOVER360) {
-
 
267
    yawGyroHeading -= YAWOVER360; // 360 deg. wrap
-
 
268
  } else if (yawGyroHeading < 0) {
-
 
269
    yawGyroHeading += YAWOVER360;
-
 
270
  }
-
 
271
 
205
  intervalWrap(&heading, YAWOVER360);
272
  /*
206
  /*
273
   * Pitch axis integration and range boundary wrap.
207
   * Pitch axis integration and range boundary wrap.
274
   */
208
   */
275
  for (axis = PITCH; axis <= ROLL; axis++) {
209
  for (axis = PITCH; axis <= ROLL; axis++) {
276
    angle[axis] += ACRate[axis];
210
    attitude[axis] += ACRate[axis];
277
    if (angle[axis] > PITCHROLLOVER180) {
211
    if (attitude[axis] > PITCHROLLOVER180) {
278
      angle[axis] -= PITCHROLLOVER360;
212
      attitude[axis] -= PITCHROLLOVER360;
279
    } else if (angle[axis] <= -PITCHROLLOVER180) {
213
    } else if (attitude[axis] <= -PITCHROLLOVER180) {
280
      angle[axis] += PITCHROLLOVER360;
214
      attitude[axis] += PITCHROLLOVER360;
281
    }
215
    }
282
  }
216
  }
Line 283... Line 217...
283
}
217
}
Line 297... Line 231...
297
  int32_t temp;
231
  int32_t temp;
Line 298... Line 232...
298
 
232
 
299
  uint8_t ca = controlActivity >> 8;
233
  uint8_t ca = controlActivity >> 8;
Line 300... Line 234...
300
  uint8_t highControlActivity = (ca > staticParams.maxControlActivity);
234
  uint8_t highControlActivity = (ca > staticParams.maxControlActivity);
301
 
235
 
302
        if (highControlActivity) {
236
  if (highControlActivity) {
303
      debugOut.digital[1] |= DEBUG_ACC0THORDER;
237
    debugOut.digital[1] |= DEBUG_ACC0THORDER;
304
        } else {
238
  } else {
Line 305... Line 239...
305
          debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
239
    debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
306
        }
240
  }
307
 
241
 
308
  if (accVector <= dynamicParams.maxAccVector) {
242
  if (accVector <= dynamicParams.maxAccVector) {
309
    debugOut.digital[0] &= ~ DEBUG_ACC0THORDER;
243
    debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
Line 310... Line 244...
310
 
244
 
311
    uint8_t permilleAcc = staticParams.zerothOrderCorrection;
245
    uint8_t permilleAcc = staticParams.zerothOrderCorrection;
312
    int32_t accDerived;
246
    int32_t accDerived;
313
 
247
 
314
    /*
248
    /*
315
    if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
249
     if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
316
      permilleAcc /= 2;
250
     permilleAcc /= 2;
317
      debugFullWeight = 0;
251
     debugFullWeight = 0;
318
    }
252
     }
319
 
253
 
Line 320... Line 254...
320
    if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
254
     if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
321
      permilleAcc /= 2;
255
     permilleAcc /= 2;
322
      debugFullWeight = 0;
256
     debugFullWeight = 0;
323
    */
257
     */
324
 
258
 
325
    if (highControlActivity) { // reduce effect during stick control activity
259
    if (highControlActivity) { // reduce effect during stick control activity
Line 326... Line 260...
326
      permilleAcc /= 4;
260
      permilleAcc /= 4;
327
      if (controlActivity > staticParams.maxControlActivity*2) { // reduce effect during stick control activity
261
      if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity
328
        permilleAcc /= 4;
262
        permilleAcc /= 4;
329
      }
263
      }
330
    }
264
    }
331
 
265
 
332
    /*
266
    /*
333
     * Add to each sum: The amount by which the angle is changed just below.
267
     * Add to each sum: The amount by which the angle is changed just below.
334
     */
268
     */
335
    for (axis = PITCH; axis <= ROLL; axis++) {
269
    for (axis = PITCH; axis <= ROLL; axis++) {
336
      accDerived = getAngleEstimateFromAcc(axis);
270
      accDerived = getAngleEstimateFromAcc(axis);
337
      debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
271
      debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
338
      // 1000 * the correction amount that will be added to the gyro angle in next line.
272
      // 1000 * the correction amount that will be added to the gyro angle in next line.
339
      temp = angle[axis];
273
      temp = attitude[axis];
340
      angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
274
      attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp
341
          + (int32_t) permilleAcc * accDerived) / 1000L;
275
          + (int32_t) permilleAcc * accDerived) / 1000L;
Line 370... Line 304...
370
 
304
 
371
  if (!--timer) {
305
  if (!--timer) {
372
    timer = DRIFTCORRECTION_TIME;
306
    timer = DRIFTCORRECTION_TIME;
373
    for (axis = PITCH; axis <= ROLL; axis++) {
307
    for (axis = PITCH; axis <= ROLL; axis++) {
374
      // Take the sum of corrections applied, add it to delta
308
      // Take the sum of corrections applied, add it to delta
375
      if (correctionSum[axis] >=0)
309
      if (correctionSum[axis] >= 0)
376
        round = DRIFTCORRECTION_TIME / 2;
310
        round = DRIFTCORRECTION_TIME / 2;
377
      else
311
      else
378
        round = -DRIFTCORRECTION_TIME / 2;
312
        round = -DRIFTCORRECTION_TIME / 2;
379
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
313
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
380
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
314
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
381
      driftComp[axis] += deltaCorrection / staticParams.driftCompDivider;
315
      driftComp[axis] += deltaCorrection / staticParams.driftCompDivider;
382
      CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit);
316
      CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit);
383
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
317
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
384
      // DebugOut.Analog[16 + axis] = correctionSum[axis];
318
      // DebugOut.Analog[16 + axis] = correctionSum[axis];
385
      // debugOut.analog[28 + axis] = driftComp[axis];
-
 
386
 
319
      // debugOut.analog[28 + axis] = driftComp[axis];
387
      correctionSum[axis] = 0;
320
      correctionSum[axis] = 0;
388
    }
321
    }
389
  }
322
  }
Line 390... Line 323...
390
}
323
}
391
 
324
 
392
void calculateAccVector(void) {
325
void calculateAccVector(void) {
393
        int16_t temp;
326
  int16_t temp;
394
        temp = filteredAcc[0] >> 3;
327
  temp = filteredAcc[0] >> 3;
395
        accVector = temp * temp;
328
  accVector = temp * temp;
396
        temp = filteredAcc[1] >> 3;
329
  temp = filteredAcc[1] >> 3;
397
        accVector += temp * temp;
330
  accVector += temp * temp;
398
        temp = filteredAcc[2] >> 3;
331
  temp = filteredAcc[2] >> 3;
-
 
332
  accVector += temp * temp;
-
 
333
  //debugOut.analog[18] = accVector;
-
 
334
}
-
 
335
 
-
 
336
void attitude_resetHeadingToMagnetic(void) {
-
 
337
  if (commands_isCalibratingCompass())
-
 
338
    return;
-
 
339
 
-
 
340
  // Compass is off, skip.
-
 
341
  if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))
-
 
342
      return;
-
 
343
 
-
 
344
  // Compass is invalid, skip.
-
 
345
  if (magneticHeading < 0)
-
 
346
    return;
-
 
347
 
-
 
348
  heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
-
 
349
  targetHeading = heading;
-
 
350
 
-
 
351
  debugOut.digital[0] ^= DEBUG_COMPASS;
-
 
352
}
-
 
353
 
-
 
354
void correctHeadingToMagnetic(void) {
-
 
355
  int32_t error;
-
 
356
 
-
 
357
  debugOut.analog[27] = heading;
-
 
358
 
-
 
359
  if (commands_isCalibratingCompass())
-
 
360
    return;
-
 
361
 
-
 
362
  // Compass is off, skip.
-
 
363
  // Naaah this is assumed.
-
 
364
  // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))
-
 
365
  //     return;
-
 
366
 
-
 
367
  // Compass is invalid, skip.
-
 
368
  if (magneticHeading < 0)
-
 
369
    return;
-
 
370
 
-
 
371
  // Spinning fast, skip
-
 
372
  if (abs(yawRate) > 128)
-
 
373
    return;
-
 
374
 
-
 
375
  // Otherwise invalidated, skip
-
 
376
  if (ignoreCompassTimer) {
-
 
377
    ignoreCompassTimer--;
-
 
378
    return;
-
 
379
  }
-
 
380
 
-
 
381
  // TODO: Find computational cost of this.
-
 
382
  error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW;
-
 
383
 
-
 
384
  // We only correct errors larger than the resolution of the compass, or else we would keep rounding the
-
 
385
  // better resolution of the gyros to the worse resolution of the compass all the time.
-
 
386
  // The correction should really only serve to compensate for gyro drift.
-
 
387
  if(abs(error) < GYRO_DEG_FACTOR_YAW) return;
-
 
388
 
-
 
389
  int32_t correction = (error * (int32_t)dynamicParams.compassYawEffect) >> 8;
-
 
390
 
-
 
391
  // The correction is added both to current heading (the direction in which the copter thinks it is pointing)
-
 
392
  // and to the target heading (the direction to which it maneuvers to point). That means, this correction has
-
 
393
  // no effect on control at all!!! It only has effect on the values of the two variables. However, these values
-
 
394
  // could have effect on control elsewhere, like in compassControl.c .
-
 
395
  heading += correction;
-
 
396
  intervalWrap(&heading, YAWOVER360);
-
 
397
 
-
 
398
  targetHeading += correction;
-
 
399
  intervalWrap(&targetHeading, YAWOVER360);
399
        accVector += temp * temp;
400
 
Line 400... Line 401...
400
        //debugOut.analog[18] = accVector;
401
  debugOut.digital[1] ^= DEBUG_COMPASS;
401
}
402
}
402
 
403
 
Line 414... Line 415...
414
#endif
415
#endif
Line 415... Line 416...
415
 
416
 
416
  // We are done reading variables from the analog module.
417
  // We are done reading variables from the analog module.
417
  // Interrupt-driven sensor reading may restart.
418
  // Interrupt-driven sensor reading may restart.
418
  startAnalogConversionCycle();
-
 
419
}
-
 
420
 
-
 
421
void updateCompass(void) {
-
 
Line 422... Line 419...
422
  int16_t w, v, r, correction, error;
419
  startAnalogConversionCycle();
423
 
-
 
424
  if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) {
-
 
425
    if (controlMixer_testCompassCalState()) {
-
 
426
      compassCalState++;
-
 
427
      if (compassCalState < 5)
-
 
428
        beepNumber(compassCalState);
-
 
429
      else
-
 
430
        beep(1000);
-
 
431
    }
-
 
432
  } else {
-
 
433
    // get maximum attitude angle
-
 
434
    w = abs(angle[PITCH] / 512);
-
 
435
    v = abs(angle[ROLL] / 512);
-
 
436
    if (v > w)
420
 
437
      w = v;
-
 
438
    correction = w / 8 + 1;
-
 
439
    // calculate the deviation of the yaw gyro heading and the compass heading
-
 
440
    if (magneticHeading < 0)
-
 
441
      error = 0; // disable yaw drift compensation if compass heading is undefined
-
 
442
    else if (abs(yawRate) > 128) { // spinning fast
-
 
443
      error = 0;
-
 
444
    } else {
-
 
445
      // compassHeading - yawGyroHeading, on a -180..179 deg interval.
-
 
446
      error = ((540 + magneticHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW))
-
 
447
          % 360) - 180;
-
 
448
    }
-
 
449
    if (!ignoreCompassTimer && w < 25) {
-
 
450
      yawGyroDrift += error;
-
 
451
      // Basically this gets set if we are in "fix" mode, and when starting.
-
 
452
      if (updateCompassCourse) {
-
 
453
        beep(200);
-
 
454
        yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
-
 
455
        compassCourse = magneticHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
-
 
456
        updateCompassCourse = 0;
-
 
457
      }
-
 
458
    }
-
 
459
    yawGyroHeading += (error * 8) / correction;
-
 
460
 
-
 
461
    /*
-
 
462
     w = (w * dynamicParams.CompassYawEffect) / 32;
-
 
463
     w = dynamicParams.CompassYawEffect - w;
-
 
464
     */
-
 
465
    w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect)
-
 
466
        / 32;
-
 
467
 
-
 
468
    // As readable formula:
-
 
469
    // w = dynamicParams.CompassYawEffect * (1-w/32);
-
 
470
 
-
 
471
    if (w >= 0) { // maxAttitudeAngle < 32
-
 
472
      if (!ignoreCompassTimer) {
-
 
473
        /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/
-
 
474
        v = 64 + controlActivity / 100;
-
 
475
        // yawGyroHeading - compassCourse on a -180..179 degree interval.
-
 
476
        r
-
 
477
            = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse)
-
 
478
                % 360) - 180;
-
 
479
        v = (r * w) / v; // align to compass course
-
 
480
        // limit yaw rate
-
 
481
        w = 3 * dynamicParams.compassYawEffect;
-
 
482
        if (v > w)
-
 
483
          v = w;
-
 
484
        else if (v < -w)
-
 
485
          v = -w;
-
 
486
        yawAngleDiff += v;
-
 
487
      } else { // wait a while
-
 
488
        ignoreCompassTimer--;
-
 
489
      }
-
 
490
    } else { // ignore compass at extreme attitudes for a while
-
 
491
      ignoreCompassTimer = 500;
421
  if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
492
    }
422
    correctHeadingToMagnetic();
Line 493... Line 423...
493
  }
423
  }
494
}
424
}