#include "sensors.h"
#include "printf_P.h"
#include "analog.h"
#include "twimaster.h"
#include "configuration.h"
#include "eeprom.h"
#include "timer0.h"
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510
#define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515
const uint8_t GYRO_REVERSED
[3] = { 0, 0, 1 };
const uint8_t ACC_REVERSED
[3] = { 0, 1, 0 };
void gyro_calibrate
(void) {
printf("gyro_calibrate");
uint8_t i
, axis
, factor
, numberOfAxesInRange
= 0;
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
for (i
= 140; i
!= 0; i
--) {
// If all 3 axis are in range, shorten the remaining number of iterations.
if (numberOfAxesInRange
== 3 && i
> 10)
i
= 10;
numberOfAxesInRange
= 0;
for (axis
= PITCH
; axis
<= YAW
; axis
++) {
if (axis
== YAW
)
factor
= GYRO_SUMMATION_FACTOR_YAW
;
else
factor
= GYRO_SUMMATION_FACTOR_PITCHROLL
;
if (rawGyroSum
[axis
] < 510 * factor
)
gyroAmplifierOffset.
offsets[axis
]--;
else if (rawGyroSum
[axis
] > 515 * factor
)
gyroAmplifierOffset.
offsets[axis
]++;
else
numberOfAxesInRange
++;
/* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */
if (gyroAmplifierOffset.
offsets[axis
] < 10) {
gyroAmplifierOffset.
offsets[axis
] = 10;
} else if (gyroAmplifierOffset.
offsets[axis
] > 245) {
gyroAmplifierOffset.
offsets[axis
] = 245;
}
}
gyro_loadOffsets
(0);
delay_ms_with_adc_measurement
(i
<= 10 ? 10 : 2);
}
gyroAmplifierOffset_writeToEEProm
();
delay_ms_with_adc_measurement
(70);
}
void gyro_loadAmplifierOffsets
(uint8_t overwriteWithDefaults
) {
uint16_t timeout
= setDelay
(2000);
if (overwriteWithDefaults
) {
gyroAmplifierOffset.
offsets[PITCH
] =
gyroAmplifierOffset.
offsets[ROLL
] =
gyroAmplifierOffset.
offsets[YAW
] = (uint8_t)(255 * 1.2089 / 3.0);
}
I2C_Start
(TWI_STATE_GYRO_OFFSET_TX
); // initiate data transmission
// Wait for I2C to finish transmission.
while (twi_state
) {
// Did it take too long?
if (checkDelay
(timeout
)) {
printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl");
break;
}
}
}
void gyro_setDefaultParameters
(void) {
staticParams.
gyroD = 3;
staticParams.
driftCompDivider = 1;
staticParams.
driftCompLimit = 200;
staticParams.
zerothOrderCorrection = 25;
}