#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
void I2C_OutputAmplifierOffsets
(void) {
uint16_t timeout
= setDelay
(2000);
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_calibrate
(void) {
printf("gyro_calibrate");
uint8_t i
, axis
, factor
, numberOfAxesInRange
= 0;
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
for (i
= 140; i
!= 0; i
--) {
delay_ms_with_adc_measurement
(i
<= 10 ? 10 : 2, 1);
// 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_OVERSAMPLING_YAW
;
else
factor
= GYRO_OVERSAMPLING_PITCHROLL
;
if (rawGyroValue
(axis
) < 510 * factor
)
gyroAmplifierOffset.
offsets[axis
]--;
else if (rawGyroValue
(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;
versionInfo.
hardwareErrors[0] |= (FC_ERROR0_GYRO_PITCH
<< axis
);
} else if (gyroAmplifierOffset.
offsets[axis
] > 245) {
gyroAmplifierOffset.
offsets[axis
] = 245;
versionInfo.
hardwareErrors[0] |= (FC_ERROR0_GYRO_PITCH
<< axis
);
}
}
I2C_OutputAmplifierOffsets
();
}
gyroAmplifierOffset_writeToEEProm
();
delay_ms_with_adc_measurement
(70, 0);
}
void gyro_init
(void) {
if (gyroAmplifierOffset_readFromEEProm
()) {
printf("gyro amp invalid, recalibrate.");
gyroAmplifierOffset.
offsets[PITCH
] =
gyroAmplifierOffset.
offsets[ROLL
] =
gyroAmplifierOffset.
offsets[YAW
] = (uint8_t)(255 * 1.2089 / 3.0);
} else {
I2C_OutputAmplifierOffsets
();
}
}
void gyro_setDefaultParameters
(void) {
IMUConfig.
gyroQuadrant = 0;
IMUConfig.
accQuadrant = 4;
IMUConfig.
imuReversedFlags = IMU_REVERSE_GYRO_YAW
| IMU_REVERSE_ACC_XY
;
staticParams.
gyroD = 3;
IMUConfig.
driftCompDivider = 1;
IMUConfig.
driftCompLimit = 200;
IMUConfig.
zerothOrderCorrection = 120;
}