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) { if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration // GYRO calibration
gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); gyroSetCalibrationCycles();
#ifdef GPS #ifdef GPS
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {

2
src/main/main.c
View file

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

2
src/main/mw.c
View file

@ -377,7 +377,7 @@ void mwArm(void)
static bool firstArmingCalibrationWasCompleted; static bool firstArmingCalibrationWasCompleted;
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) { if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); gyroSetCalibrationCycles();
armingCalibrationWasInitialised = true; armingCalibrationWasInitialised = true;
firstArmingCalibrationWasCompleted = 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]; int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[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 int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig; static const gyroConfig_t *gyroConfig;
static biquad_t gyroFilterState[3]; static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
static uint8_t gyroLpfHz; 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; gyroConfig = gyroConfigToUse;
gyroLpfHz = gyro_lpf_hz; gyroSoftLpfHz = gyro_soft_lpf_hz;
} }
void gyroInit(void) 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++) { 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) bool isGyroCalibrationComplete(void)
{ {
return calibratingG == 0; return calibratingG == 0;
@ -77,7 +72,7 @@ static bool isOnFinalGyroCalibrationCycle(void)
return calibratingG == 1; return calibratingG == 1;
} }
uint16_t gyroCalculateCalibratingCycles(void) static uint16_t gyroCalculateCalibratingCycles(void)
{ {
return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
} }
@ -87,6 +82,11 @@ static bool isOnFirstGyroCalibrationCycle(void)
return calibratingG == gyroCalculateCalibratingCycles(); return calibratingG == gyroCalculateCalibratingCycles();
} }
void gyroSetCalibrationCycles(void)
{
calibratingG = gyroCalculateCalibratingCycles();
}
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold) static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
{ {
static int32_t g[3]; static int32_t g[3];
@ -112,7 +112,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
float dev = devStandardDeviation(&var[axis]); float dev = devStandardDeviation(&var[axis]);
// check deviation and startover in case the model was moved // check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(gyroCalculateCalibratingCycles()); gyroSetCalibrationCycles();
return; return;
} }
gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
@ -155,7 +155,7 @@ void gyroUpdate(void)
applyGyroZero(); applyGyroZero();
if (gyroLpfHz) { if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]);
gyroADC[axis] = lrintf(gyroADCf[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. 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; } gyroConfig_t;
void gyroUseConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); void gyroSetCalibrationCycles(void);
void gyroInit(void); void gyroInit(void);
void gyroUpdate(void); void gyroUpdate(void);
bool isGyroCalibrationComplete(void); bool isGyroCalibrationComplete(void);
uint16_t gyroCalculateCalibratingCycles(void);