Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

Improved gyroSetCalibrationCycles parameters.

Browse source
This commit is contained in:
Martin Budden 2016-06-26 21:43:04 +01:00
parent 75237dd209
commit d069945f89
5 changed files with 21 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

32
src/main/sensors/gyro.c
View file

@ -41,32 +41,27 @@ sensor_align_e gyroAlign = 0;
int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[XYZ_AXIS_COUNT];
static uint16_t calibratingG = 0;
static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static biquad_t gyroFilterState[3];
static uint8_t gyroLpfHz;
static const gyroConfig_t *gyroConfig;
static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz)
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
{
gyroConfig = gyroConfigToUse;
gyroLpfHz = gyro_lpf_hz;
gyroSoftLpfHz = gyro_soft_lpf_hz;
}
void gyroInit(void)
{
if (gyroLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
BiQuadNewLpf(gyroLpfHz, &gyroFilterState[axis], gyro.targetLooptime);
BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime);
}
}
}
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibratingG = calibrationCyclesRequired;
}
bool isGyroCalibrationComplete(void)
{
return calibratingG == 0;
@ -77,7 +72,7 @@ static bool isOnFinalGyroCalibrationCycle(void)
return calibratingG == 1;
}
uint16_t gyroCalculateCalibratingCycles(void)
static uint16_t gyroCalculateCalibratingCycles(void)
{
return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
}
@ -87,6 +82,11 @@ static bool isOnFirstGyroCalibrationCycle(void)
return calibratingG == gyroCalculateCalibratingCycles();
}
void gyroSetCalibrationCycles(void)
{
calibratingG = gyroCalculateCalibratingCycles();
}
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
{
static int32_t g[3];
@ -112,7 +112,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
float dev = devStandardDeviation(&var[axis]);
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(gyroCalculateCalibratingCycles());
gyroSetCalibrationCycles();
return;
}
gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
@ -155,7 +155,7 @@ void gyroUpdate(void)
applyGyroZero();
if (gyroLpfHz) {
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);