98 lines
2.5 KiB
C
98 lines
2.5 KiB
C
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#include "common/axis.h"
|
|
#include "common/maths.h"
|
|
|
|
#include "drivers/accgyro_common.h"
|
|
#include "flight_common.h"
|
|
#include "sensors_common.h"
|
|
#include "statusindicator.h"
|
|
#include "boardalignment.h"
|
|
|
|
#include "sensors_gyro.h"
|
|
|
|
uint16_t calibratingG = 0;
|
|
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
|
|
gyro_t gyro; // gyro access functions
|
|
sensor_align_e gyroAlign = 0;
|
|
|
|
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
|
{
|
|
calibratingG = calibrationCyclesRequired;
|
|
}
|
|
|
|
bool isGyroCalibrationComplete(void)
|
|
{
|
|
return calibratingG == 0;
|
|
}
|
|
|
|
bool isOnFinalGyroCalibrationCycle(void)
|
|
{
|
|
return calibratingG == 1;
|
|
}
|
|
|
|
bool isOnFirstGyroCalibrationCycle(void)
|
|
{
|
|
return calibratingG == CALIBRATING_GYRO_CYCLES;
|
|
}
|
|
|
|
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 if idiot was moving the model
|
|
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 void applyGyroZero(void)
|
|
{
|
|
int8_t axis;
|
|
for (axis = 0; axis < 3; axis++) {
|
|
gyroADC[axis] -= gyroZero[axis];
|
|
}
|
|
}
|
|
|
|
void gyroGetADC(uint8_t gyroMovementCalibrationThreshold)
|
|
{
|
|
// range: +/- 8192; +/- 2000 deg/sec
|
|
gyro.read(gyroADC);
|
|
alignSensors(gyroADC, gyroADC, gyroAlign);
|
|
|
|
if (!isGyroCalibrationComplete()) {
|
|
performAcclerationCalibration(gyroMovementCalibrationThreshold);
|
|
}
|
|
|
|
applyGyroZero();
|
|
}
|