static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
{
int8_t axis;
static int32_t g[3];
static stdev_t var[3];
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle()) {
g[axis] = 0;
devClear(&var[axis]);
}
// Sum up CALIBRATING_GYRO_CYCLES readings
g[axis] += gyroADC[axis];
devPush(&var[axis], gyroADC[axis]);
// Reset global variables to prevent other code from using un-calibrated data
gyroADC[axis] = 0;
gyroZero[axis] = 0;
if (isOnFinalGyroCalibrationCycle()) {
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
return;
}
gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
blinkLedAndSoundBeeper(10, 15, 1);
}
}
calibratingG--;
}
STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
gyroSensor->calibration.sum[axis] = 0.0f;
devClear(&gyroSensor->calibration.var[axis]);
// gyroZero is set to zero until calibration complete
gyroSensor->gyroDev.gyroZero[axis] = 0.0f;
}
// Sum up CALIBRATING_GYRO_TIME_US readings
gyroSensor->calibration.sum[axis] += gyroSensor->gyroDev.gyroADCRaw[axis];
devPush(&gyroSensor->calibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]);
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]);
// DEBUG_GYRO_CALIBRATION records the standard deviation of roll
// into the spare field - debug[3], in DEBUG_GYRO_RAW
if (axis == X) {
DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
}
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(gyroSensor);
return;
}
// please take care with exotic boardalignment !!
gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
if (axis == Z) {
gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
}
}
}
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
beeper(BEEPER_GYRO_CALIBRATED);
}
}
--gyroSensor->calibration.cyclesRemaining;
}
static void GYRO_Common(void)
{
int axis;
static int32_t g[3];
static stdev_t var[3];
if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == CALIBRATING_GYRO_CYCLES) {
g[axis] = 0;
devClear(&var[axis]);
}
// Sum up 1000 readings
g[axis] += gyroADC[axis];
devPush(&var[axis], gyroADC[axis]);
// Clear global variables for next reading
gyroADC[axis] = 0;
gyroZero[axis] = 0;
if (calibratingG == 1) {
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover if idiot was moving the model
if (mcfg.moron_threshold && dev > mcfg.moron_threshold) {
calibratingG = CALIBRATING_GYRO_CYCLES;
devClear(&var[0]);
devClear(&var[1]);
devClear(&var[2]);
g[0] = g[1] = g[2] = 0;
continue;
}
//gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
gyroZero[axis] = (g[axis]+1) / CALIBRATING_GYRO_CYCLES;
blinkLED(10, 15, 1);
}
}
calibratingG--;
}
for (axis = 0; axis < 3; axis++)
gyroADC[axis] -= gyroZero[axis];
}
static void Gyro_Calibrate(void) // Total Samples = Gyromaxcount * Gyroavgcount + Gyrodiscardcnt
{
float Temp[3];
uint16_t i, axis;
uint8_t breakout = 0;
stdev_t var[3];
Gyro_500Hz_AVG(Temp, Gyrodiscardcnt); // Discard some values here to let gyro settle
do // Shaky Hands Loop NOTE: Removed Sphere stuff here because results are equal!
{
breakout++; // Increase Breakout Counter
for (axis = 0; axis < 3; axis++) // Clear for Run
{
devClear(&var[axis]);
gyroZero[axis] = 0.0f;
}
for (i = 0; i < Gyromaxcount; i++) // Outer loop for StdDev
{
Gyro_500Hz_AVG(Temp, Gyroavgcount); // Average some
for (axis = 0; axis < 3; axis++) // Add StdDev, save values for next loopturn
{
devPush(&var[axis], Temp[axis]);
gyroZero[axis] += Temp[axis];
}
}
Temp[0] = 0.0f;
for (axis = 0; axis < 3; axis++)
{
gyroZero[axis] /= (float)Gyromaxcount; // Calculate gyrozero no matter what. Not timecritical anyway
Temp[0] += devStandardDeviation(&var[axis]);
}
}
while (Temp[0] > cfg.gy_stdev && breakout < Timeoutrun); // Breakout prevents endlessloop after time
GyroCalCompromised = (breakout == Timeoutrun); // We timed out so gyro is problematic
if(GyroCalCompromised && cfg.ShakyDataAvail) // Problem? Use backupdata, if available (normally they are..)
{
for (axis = 0; axis < 3; axis++) gyroZero[axis] = cfg.ShakyGyroZero[axis];
blinkLED(15, 20, 10); // Warnblink Flight may be degraded but possible.
}
}
void IMU::update(uint32_t currentTime, bool armed, uint16_t & calibratingA, uint16_t & calibratingG)
{
static float accelLPF[3];
static int32_t accelZoffset;
static float accz_smooth;
static int16_t accelZero[3];
static int32_t a[3];
static int16_t accelSmooth[3];
static float EstG[3];
static float EstN[3] = { 1.0f, 0.0f, 0.0f };
static int16_t gyroZero[3];
static uint32_t previousTime;
int32_t accMag = 0;
float dT = 0;
float rpy[3];
float accel_ned[3];
float deltaGyroAngle[3];
uint32_t deltaT = currentTime - previousTime;
float scale = deltaT * this->gyroScale;
int16_t accelADC[3];
float anglerad[3];
previousTime = currentTime;
this->_board->imuRead(accelADC, this->gyroADC);
if (calibratingA > 0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == this->calibratingAccCycles)
a[axis] = 0;
// Sum up this->calibratingAccCycles readings
a[axis] += accelADC[axis];
// Clear global variables for next reading
accelADC[axis] = 0;
accelZero[axis] = 0;
}
// Calculate average, shift Z down by acc1G
if (calibratingA == 1) {
accelZero[ROLL] = (a[ROLL] + (this->calibratingAccCycles / 2)) / this->calibratingAccCycles;
accelZero[PITCH] = (a[PITCH] + (this->calibratingAccCycles / 2)) / this->calibratingAccCycles;
accelZero[YAW] = (a[YAW] + (this->calibratingAccCycles / 2)) / this->calibratingAccCycles - this->acc1G;
}
calibratingA--;
}
accelADC[ROLL] -= accelZero[ROLL];
accelADC[PITCH] -= accelZero[PITCH];
accelADC[YAW] -= accelZero[YAW];
// range: +/- 8192; +/- 2000 deg/sec
static int32_t g[3];
static stdev_t var[3];
if (calibratingG > 0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (calibratingG == this->calibratingGyroCycles) {
g[axis] = 0;
devClear(&var[axis]);
}
// Sum up 1000 readings
g[axis] += this->gyroADC[axis];
devPush(&var[axis], this->gyroADC[axis]);
// Clear global variables for next reading
this->gyroADC[axis] = 0;
gyroZero[axis] = 0;
if (calibratingG == 1) {
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover if idiot was moving the model
if (CONFIG_MORON_THRESHOLD && dev > CONFIG_MORON_THRESHOLD) {
calibratingG = this->calibratingGyroCycles;
devClear(&var[0]);
devClear(&var[1]);
devClear(&var[2]);
g[0] = g[1] = g[2] = 0;
continue;
}
gyroZero[axis] = (g[axis] + (this->calibratingGyroCycles / 2)) / this->calibratingGyroCycles;
}
}
calibratingG--;
}
for (uint8_t axis = 0; axis < 3; axis++)
this->gyroADC[axis] -= gyroZero[axis];
// Initialization
for (uint8_t axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = this->gyroADC[axis] * scale;
if (CONFIG_ACC_LPF_FACTOR > 0) {
accelLPF[axis] = accelLPF[axis] * (1.0f - (1.0f / CONFIG_ACC_LPF_FACTOR)) + accelADC[axis] *
(1.0f / CONFIG_ACC_LPF_FACTOR);
accelSmooth[axis] = (int16_t)accelLPF[axis];
} else {
accelSmooth[axis] = accelADC[axis];
}
accMag += (int32_t)accelSmooth[axis] * accelSmooth[axis];
}
accMag = accMag * 100 / ((int32_t)this->acc1G * this->acc1G);
rotateV(EstG, deltaGyroAngle);
// Apply complementary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit
// range => we neutralize the effect of accelerometers in the angle
// estimation. To do that, we just skip filter, as EstV already rotated by Gyro
if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
for (uint8_t axis = 0; axis < 3; axis++)
EstG[axis] = (EstG[axis] * (float)CONFIG_GYRO_CMPF_FACTOR + accelSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
// Attitude of the estimated vector
anglerad[ROLL] = atan2f(EstG[Y], EstG[Z]);
anglerad[PITCH] = atan2f(-EstG[X], sqrtf(EstG[Y] * EstG[Y] + EstG[Z] * EstG[Z]));
rotateV(EstN, deltaGyroAngle);
normalizeV(EstN, EstN);
// Calculate heading
float cosineRoll = cosf(anglerad[ROLL]);
float sineRoll = sinf(anglerad[ROLL]);
float cosinePitch = cosf(anglerad[PITCH]);
float sinePitch = sinf(anglerad[PITCH]);
float Xh = EstN[X] * cosinePitch + EstN[Y] * sineRoll * sinePitch + EstN[Z] * sinePitch * cosineRoll;
float Yh = EstN[Y] * cosineRoll - EstN[Z] * sineRoll;
anglerad[YAW] = atan2f(Yh, Xh);
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
// the accel values have to be rotated into the earth frame
rpy[0] = -(float)anglerad[ROLL];
rpy[1] = -(float)anglerad[PITCH];
rpy[2] = -(float)anglerad[YAW];
accel_ned[X] = accelSmooth[0];
accel_ned[Y] = accelSmooth[1];
accel_ned[Z] = accelSmooth[2];
rotateV(accel_ned, rpy);
if (!armed) {
accelZoffset -= accelZoffset / 64;
accelZoffset += (int32_t)accel_ned[Z];
}
accel_ned[Z] -= accelZoffset / 64; // compensate for gravitation on z-axis
accz_smooth = accz_smooth + (dT / (fcAcc + dT)) * (accel_ned[Z] - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence and
// sum up Values for later integration to get velocity and distance
this->accelSum[X] += deadbandFilter(lrintf(accel_ned[X]), CONFIG_ACCXY_DEADBAND);
this->accelSum[Y] += deadbandFilter(lrintf(accel_ned[Y]), CONFIG_ACCXY_DEADBAND);
this->accelSum[Z] += deadbandFilter(lrintf(accz_smooth), CONFIG_ACCZ_DEADBAND);
this->accelTimeSum += deltaT;
this->accelSumCount++;
// Convert angles from radians to tenths of a degrees
this->angle[ROLL] = (int16_t)lrintf(anglerad[ROLL] * (1800.0f / M_PI));
this->angle[PITCH] = (int16_t)lrintf(anglerad[PITCH] * (1800.0f / M_PI));
this->angle[YAW] = (int16_t)(lrintf(anglerad[YAW] * 1800.0f / M_PI + CONFIG_MAGNETIC_DECLINATION) / 10.0f);
// Convert heading from [-180,+180] to [0,360]
if (this->angle[YAW] < 0)
this->angle[YAW] += 360;
}
