Subversion Repositories FlightCtrl

Rev

Rev 2112 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2112 Rev 2189
1 
#include "sensors.h"
 1 
#include "sensors.h"
2 
#include "printf_P.h"
 - 
 
3 
#include "analog.h"
 2 
#include "analog.h"
4 
#include "twimaster.h"
 3 
#include "twimaster.h"
5 
#include "configuration.h"
 4 
#include "configuration.h"
6 
#include "eeprom.h"
 5 
#include "eeprom.h"
7 
#include "timer0.h"
 6 
#include "timer0.h"
- 
 
 7 
#include "output.h"
- 
 
 8 
 
- 
 
 9 
#include <stdio.h>
8 
 
 10 
 
9 
void I2C_OutputAmplifierOffsets(void) {
 11 
void I2C_OutputAmplifierOffsets(void) {
10 
        uint16_t timeout = setDelay(2000);
 12 
        uint16_t timeout = setDelay(2000);
11 
        I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission
 13 
        I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission
12 
        // Wait for I2C to finish transmission.
 14 
        // Wait for I2C to finish transmission.
13 
        while (twi_state) {
 15 
        while (twi_state) {
14 
                // Did it take too long?
 16 
                // Did it take too long?
15 
                if (checkDelay(timeout)) {
 17 
                if (checkDelay(timeout)) {
16 
                        printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl");
 18 
                        printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl");
17 
                        break;
 19 
                        break;
18 
                }
 20 
                }
19 
        }
 21 
        }
20 
}
 22 
}
- 
 
 23 
 
- 
 
 24 
uint8_t DACChannelFor(uint8_t axis) {
- 
 
 25 
        switch (axis) {
- 
 
 26 
        case X:
- 
 
 27 
                return 1;
- 
 
 28 
    case Y:
- 
 
 29 
        return 0;
- 
 
 30 
        case Z:
- 
 
 31 
                return 2;
- 
 
 32 
        default:
- 
 
 33 
                return -1; // should never happen.
- 
 
 34 
        }
- 
 
 35 
}
21 
 
 36 
 
22 
void gyro_calibrate(void) {                            
 37 
void gyro_calibrate(void) {                            
23 
        printf("gyro_calibrate");
 38 
        printf("gyro_calibrate");
24 
        uint8_t i, axis, factor, numberOfAxesInRange = 0;
 39 
        uint8_t i, axis, factor, numberOfAxesInRange = 0;
25 
        // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
 40 
        // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
26 
       
 41 
 
27 
        for (i = 140; i != 0; i--) {
 42 
        for (i = 200; i != 0; i--) {
28 
                delay_ms_with_adc_measurement(i <= 10 ? 10 : 2, 1);
 43 
                waitADCCycle(i <= 10 ? 10 : 2);
29 
       
 - 
 
30 
                // If all 3 axis are in range, shorten the remaining number of iterations.
 44 
                // If all 3 axis are in range, shorten the remaining number of iterations.
31 
                if (numberOfAxesInRange == 3 && i > 10) i = 10;
 45 
                if (numberOfAxesInRange == 3 && i > 10) i = 10;
32 
                       
 - 
 
33 
                numberOfAxesInRange = 0;
 46 
                numberOfAxesInRange = 0;
34 
 
 - 
 
35 
                for (axis = PITCH; axis <= YAW; axis++) {
 47 
                for (axis=X; axis<=Z; axis++) {
- 
 
 48 
                        uint8_t dacChannel = DACChannelFor(axis);
36 
                        if (axis == YAW)
 49 
                        if (axis==Z)
37 
                                factor = GYRO_OVERSAMPLING_YAW;
 50 
                                factor=GYRO_OVERSAMPLING_Z;
38 
                        else
 51 
                        else
39 
                                factor = GYRO_OVERSAMPLING_PITCHROLL;
 52 
                                factor=GYRO_OVERSAMPLING_XY;
40 
 
 53 
 
41 
                        if (rawGyroValue(axis) < 510 * factor)
 54 
                        if (gyroValueForFC13DACCalibration(axis) < (uint16_t)(510*factor))
42 
                                gyroAmplifierOffset.offsets[axis]--;
 55 
                                gyroAmplifierOffset.offsets[dacChannel]--;
43 
                        else if (rawGyroValue(axis) > 515 * factor)
 56 
                        else if (gyroValueForFC13DACCalibration(axis) > (uint16_t)(515 * factor))
44 
                                gyroAmplifierOffset.offsets[axis]++;
 57 
                                gyroAmplifierOffset.offsets[dacChannel]++;
45 
                        else
 58 
                        else
46 
                                numberOfAxesInRange++;
 59 
                                numberOfAxesInRange++;
47 
 
 60 
 
48 
                        /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */
 61 
                        /* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */
49 
                        if (gyroAmplifierOffset.offsets[axis] < 10) {
 62 
                        if (gyroAmplifierOffset.offsets[dacChannel] < 10) {
50 
                                gyroAmplifierOffset.offsets[axis] = 10;
 63 
                                gyroAmplifierOffset.offsets[dacChannel] = 10;
51 
                                versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_PITCH << axis);
 64 
                                versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_X << axis);
52 
                        } else if (gyroAmplifierOffset.offsets[axis] > 245) {
 65 
                        } else if (gyroAmplifierOffset.offsets[dacChannel] > 245) {
53 
                                gyroAmplifierOffset.offsets[axis] = 245;
 66 
                                gyroAmplifierOffset.offsets[dacChannel] = 245;
54 
                                versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_PITCH << axis);
 67 
                                versionInfo.hardwareErrors[0] |= (FC_ERROR0_GYRO_X << axis);
55 
                        }
 68 
                        }
56 
                }
 69 
                }
57 
               
 70 
               
58 
                I2C_OutputAmplifierOffsets();
 71 
                I2C_OutputAmplifierOffsets();
59 
        }
 72 
        }
60 
        gyroAmplifierOffset_writeToEEProm();
 73 
        gyroAmplifierOffset_writeToEEProm();
61 
        delay_ms_with_adc_measurement(70, 0);
 74 
        waitADCCycle(70);
62 
}
 75 
}
63 
 
 76 
 
64 
void gyro_init(void) {
 77 
void gyro_init(void) {
65 
  if (gyroAmplifierOffset_readFromEEProm()) {
 78 
  if (gyroAmplifierOffset_readFromEEProm()) {
66 
    printf("gyro amp invalid, recalibrate.");
 79 
    printf("gyro amp invalid, recalibrate.");
67 
        gyroAmplifierOffset.offsets[PITCH] =
 80 
        gyroAmplifierOffset.offsets[X] =
68 
                gyroAmplifierOffset.offsets[ROLL] =
 81 
                gyroAmplifierOffset.offsets[Y] =
69 
                gyroAmplifierOffset.offsets[YAW] = (uint8_t)(255 * 1.2089 / 3.0);
 82 
                gyroAmplifierOffset.offsets[Z] = (uint8_t)(255 * 1.2089 / 3.0);
70 
  } else {
 83 
  } else {
71 
        I2C_OutputAmplifierOffsets();
 84 
        I2C_OutputAmplifierOffsets();
72 
  }
 85 
  }
73 
}
 86 
}
74 
 
 87 
 
75 
void gyro_setDefaultParameters(void) {
 88 
void gyro_setDefaultParameters(void) {
76 
  IMUConfig.gyroQuadrant = 0;
 89 
  IMUConfig.gyroQuadrant = 4;
77 
  IMUConfig.accQuadrant = 4;
 90 
  IMUConfig.accQuadrant = 4;
78 
  IMUConfig.imuReversedFlags = IMU_REVERSE_GYRO_YAW | IMU_REVERSE_ACC_XY;
 91 
  IMUConfig.imuReversedFlags = IMU_REVERSE_ACCEL_Z | IMU_REVERSE_GYRO_Z;
79 
  staticParams.gyroD = 3;
 92 
  staticParams.gyroD = 3;
80 
  IMUConfig.driftCompDivider = 1;
 - 
 
81 
  IMUConfig.driftCompLimit = 200;
 - 
 
82 
  IMUConfig.zerothOrderCorrection = 120;
 - 
 
83 
}
 93 
}
84 
 
 94