Subversion Repositories FlightCtrl

Rev

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

Rev 1645 Rev 1646
Line 3... Line 3...
3 
#include "analog.h"
 3 
#include "analog.h"
4 
#include "twimaster.h"
 4 
#include "twimaster.h"
5 
#include "configuration.h"
 5 
#include "configuration.h"
6 
#include "timer0.h"
 6 
#include "timer0.h"
Line 7... Line -...
7 
 
 - 
 
8 
const uint8_t GYROS_REVERSE[2] = {0,0};
 7 
 
9 
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510
 8 
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510
Line 10... Line 9...
10 
#define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515
 9 
#define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515
11 
 
 10 
 
12 
#define DAC_PITCH 0
 - 
 
Line 13... Line 11...
13 
#define DAC_ROLL 1
 11 
const uint8_t GYRO_REVERSED[3] = {0,0,1};
14 
#define DAC_YAW 2
 12 
const uint8_t ACC_REVERSED[3] = {0,1,0};
15 
 
 13 
 
16 
// void gyro_init(void) {}
 14 
// void gyro_init(void) {}
17 
void gyro_calibrate(void) {
 15 
void gyro_calibrate(void) {
18 
  uint8_t i, numberOfAxesInRange = 0;
 16 
  uint8_t i, axis, factor, numberOfAxesInRange = 0;
Line 19... Line 17...
19 
  uint16_t timeout;
 17 
  uint16_t timeout;
20 
  // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
 18 
  // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
21 
  timeout = SetDelay(2000);
 19 
  timeout = SetDelay(2000);
22 
 
 - 
 
23 
  for(i = 140; i != 0; i--) {
 20 
 
Line 24... Line 21...
24 
    // If all 3 axis are in range, shorten the remaining number of iterations.
 21 
  for(i = 140; i != 0; i--) {
25 
    if(numberOfAxesInRange == 3 && i > 10) i = 9;
 22 
    // If all 3 axis are in range, shorten the remaining number of iterations.
26 
   
 23 
    if(numberOfAxesInRange == 3 && i > 10) i = 9;
27 
    numberOfAxesInRange = 0;
 24 
    numberOfAxesInRange = 0;
28 
   
 25 
   
29 
    if(rawGyroSum[PITCH] < PITCHROLL_MINLIMIT) DACValues[PITCH]--;
 26 
    for (axis=PITCH; axis<=YAW; axis++) {
30 
    else if(rawGyroSum[PITCH] > PITCHROLL_MAXLIMIT) DACValues[PITCH]++;
 27 
        if (axis==YAW) factor = GYRO_SUMMATION_FACTOR_YAW;
31 
    else numberOfAxesInRange++;
 28 
        else factor = GYRO_SUMMATION_FACTOR_PITCHROLL;
32 
   
 - 
 
33 
    if(rawGyroSum[ROLL] < PITCHROLL_MINLIMIT) DACValues[ROLL]--;
 29 
 
34 
    else if(rawGyroSum[ROLL] > PITCHROLL_MAXLIMIT) DACValues[ROLL]++;
 - 
 
35 
    else numberOfAxesInRange++;
 - 
 
36 
   
 30 
          if(rawGyroSum[axis] < factor * 510) DACValues[axis]--;
37 
    if(rawYawGyroSum < GYRO_SUMMATION_FACTOR_YAW * 510) DACValues[DAC_YAW]--;
 - 
 
38 
    else if(rawYawGyroSum > GYRO_SUMMATION_FACTOR_YAW * 515) DACValues[DAC_YAW]++ ;
 - 
 
39 
    else numberOfAxesInRange++;
 - 
 
40 
   
 - 
 
41 
    if(DACValues[PITCH] < 10) {
 31 
          else if(rawGyroSum[axis] > limit * 515) DACValues[axis]++;
42 
      /* GyroDefectNick = 1; */ DACValues[PITCH] = 10;
 - 
 
43 
    } else if(DACValues[PITCH] > 245) {
 32 
      else numberOfAxesInRange++;
44 
      /* GyroDefectNick = 1; */ DACValues[PITCH] = 245;
 33 
   
45 
    }
 34 
      /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp).
46 
    if(DACValues[DAC_ROLL] < 10) {
 - 
 
47 
      /* GyroDefectRoll = 1; */ DACValues[ROLL] = 10;
 - 
 
48 
    } else if(DACValues[DAC_ROLL] > 245) {
 - 
 
49 
      /* GyroDefectRoll = 1; */ DACValues[ROLL] = 245;
 - 
 
50 
    }
 35 
      if(DACValues[axis] < 10) {
Line 51... Line 36...
51 
    if(DACValues[DAC_YAW]  < 10) {
 36 
        DACValues[axis] = 10;
Line 52... Line 37...
52 
      /* GyroDefectYaw  = 1; */ DACValues[DAC_YAW]  = 10;
 37 
      } else if(DACValues[axis] > 245) {
53 
    } else if(DACValues[DAC_YAW]  > 245) {
 38 
        DACValues[axis] = 245;
54 
      /* GyroDefectYaw  = 1; */ DACValues[DAC_YAW]  = 245;
 39 
      }
55 
    }
 40 
    }
56 
 
 41 
 
57 
    I2C_Start(TWI_STATE_GYRO_OFFSET_TX);   // initiate data transmission
 42 
    I2C_Start(TWI_STATE_GYRO_OFFSET_TX);   // initiate data transmission
58 
   
 43 
   
59 
    // Wait for I2C to finish transmission.
 44 
    // Wait for I2C to finish transmission.
Line 60... Line 45...
60 
    while(twi_state) {
 45 
    while(twi_state) {
Line 71... Line 56...
71 
  }
 56 
  }
72 
  Delay_ms_Mess(70);
 57 
  Delay_ms_Mess(70);
73 
}
 58 
}
Line 74... Line 59...
74 
 
 59 
 
75 
void gyro_setDefaults(void) {
 60 
void gyro_setDefaults(void) {
76 
  staticParams.GyroD = 3;
 61 
  staticParams.GyroD         = 3;
77 
  staticParams.DriftComp = 32;
 62 
  staticParams.DriftComp     = 100;
- 
 
 63 
  staticParams.GyroAccFactor = 1;
Line 78... Line 64...
78 
  staticParams.GyroAccFactor = 5;
 64 
  staticParams.GyroAccTrim   = 5;
79 
 
 65 
 
80 
  // Not used.
 66 
  // Not used.
81 
  staticParams.AngleTurnOverPitch = 85;
 67 
  staticParams.AngleTurnOverPitch = 85;