Subversion Repositories FlightCtrl

Rev

Rev 1645 | Rev 1775 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1645 Rev 1646
1 
//#include "ENC-03_FC1.3.h"
 1 
//#include "ENC-03_FC1.3.h"
2 
#include "printf_P.h"
 2 
#include "printf_P.h"
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"
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
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
 11 
const uint8_t GYRO_REVERSED[3] = {0,0,1};
13 
#define DAC_ROLL 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;
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 
 
 20 
 
23 
  for(i = 140; i != 0; i--) {
 21 
  for(i = 140; i != 0; i--) {
24 
    // If all 3 axis are in range, shorten the remaining number of iterations.
 22 
    // If all 3 axis are in range, shorten the remaining number of iterations.
25 
    if(numberOfAxesInRange == 3 && i > 10) i = 9;
 23 
    if(numberOfAxesInRange == 3 && i > 10) i = 9;
26 
   
 - 
 
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 
   
 29 
 
33 
    if(rawGyroSum[ROLL] < PITCHROLL_MINLIMIT) DACValues[ROLL]--;
 30 
          if(rawGyroSum[axis] < factor * 510) DACValues[axis]--;
34 
    else if(rawGyroSum[ROLL] > PITCHROLL_MAXLIMIT) DACValues[ROLL]++;
 31 
          else if(rawGyroSum[axis] > limit * 515) DACValues[axis]++;
35 
    else numberOfAxesInRange++;
 32 
      else numberOfAxesInRange++;
36 
   
 - 
 
37 
    if(rawYawGyroSum < GYRO_SUMMATION_FACTOR_YAW * 510) DACValues[DAC_YAW]--;
 33 
   
38 
    else if(rawYawGyroSum > GYRO_SUMMATION_FACTOR_YAW * 515) DACValues[DAC_YAW]++ ;
 - 
 
39 
    else numberOfAxesInRange++;
 - 
 
40 
   
 34 
      /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp).
41 
    if(DACValues[PITCH] < 10) {
 - 
 
42 
      /* GyroDefectNick = 1; */ DACValues[PITCH] = 10;
 - 
 
43 
    } else if(DACValues[PITCH] > 245) {
 - 
 
44 
      /* GyroDefectNick = 1; */ DACValues[PITCH] = 245;
 - 
 
45 
    }
 35 
      if(DACValues[axis] < 10) {
46 
    if(DACValues[DAC_ROLL] < 10) {
 - 
 
47 
      /* GyroDefectRoll = 1; */ DACValues[ROLL] = 10;
 36 
        DACValues[axis] = 10;
48 
    } else if(DACValues[DAC_ROLL] > 245) {
 37 
      } else if(DACValues[axis] > 245) {
49 
      /* GyroDefectRoll = 1; */ DACValues[ROLL] = 245;
 38 
        DACValues[axis] = 245;
50 
    }
 - 
 
51 
    if(DACValues[DAC_YAW]  < 10) {
 - 
 
52 
      /* GyroDefectYaw  = 1; */ DACValues[DAC_YAW]  = 10;
 - 
 
53 
    } else if(DACValues[DAC_YAW]  > 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.
60 
    while(twi_state) {
 45 
    while(twi_state) {
61 
      // Did it take too long?
 46 
      // Did it take too long?
62 
      if(CheckDelay(timeout)) {
 47 
      if(CheckDelay(timeout)) {
63 
        printf("\r\n DAC or I2C Error1 check I2C, 3Vref, DAC, and BL-Ctrl");
 48 
        printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl");
64 
        break;
 49 
            break;
65 
      }
 50 
      }
66 
    }
 51 
    }
67 
 
 52 
 
68 
    analog_start();
 53 
    analog_start();
69 
 
 54 
 
70 
    Delay_ms_Mess(i<10 ? 10 : 2);
 55 
    Delay_ms_Mess(i<10 ? 10 : 2);
71 
  }
 56 
  }
72 
  Delay_ms_Mess(70);
 57 
  Delay_ms_Mess(70);
73 
}
 58 
}
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;
78 
  staticParams.GyroAccFactor = 5;
 63 
  staticParams.GyroAccFactor = 1;
- 
 
 64 
  staticParams.GyroAccTrim   = 5;
79 
 
 65 
 
80 
  // Not used.
 66 
  // Not used.
81 
  staticParams.AngleTurnOverPitch = 85;
 67 
  staticParams.AngleTurnOverPitch = 85;
82 
  staticParams.AngleTurnOverRoll = 85;
 68 
  staticParams.AngleTurnOverRoll = 85;
83 
}
 69 
}
84 
 
 70