Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +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

2
src/main/io/rc_controls.c
View file

@ -209,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
gyroSetCalibrationCycles(gyroCalculateCalibratingCycles());
gyroSetCalibrationCycles();
#ifdef GPS
if (feature(FEATURE_GPS)) {

2
src/main/main.c
View file

@ -625,7 +625,7 @@ void init(void)
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(gyroCalculateCalibratingCycles());
gyroSetCalibrationCycles();
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif

2
src/main/mw.c
View file

@ -377,7 +377,7 @@ void mwArm(void)
static bool firstArmingCalibrationWasCompleted;
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
gyroSetCalibrationCycles(gyroCalculateCalibratingCycles());
gyroSetCalibrationCycles();
armingCalibrationWasInitialised = true;
firstArmingCalibrationWasCompleted = true;
}

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]);

5
src/main/sensors/gyro.h
View file

@ -39,10 +39,9 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t;
void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
void gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);
uint16_t gyroCalculateCalibratingCycles(void);