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

Improved gyro initialisation (#456)

Browse source 
* Improved gyro initialisation
* Fixed acc filter initialisation. Improved detection routines
* Fixed biquad filter initialisation
* Changed gyroSoftLpfHz from int8_t to uint8_t
This commit is contained in:
Martin Budden 2016-08-29 11:44:49 +01:00 • committed by Konstantin Sharlaimov
parent 30cd4183ec
commit b45737d3b2
19 changed files with 116 additions and 114 deletions
Download patch file Download diff file Expand all files Collapse all files

45
src/main/sensors/acceleration.c
View file

@ -42,6 +42,7 @@
acc_t acc; // acc access functions
int32_t accADC[XYZ_AXIS_COUNT];
sensor_align_e accAlign = 0;
uint32_t accTargetLooptime;
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static int16_t accADCRaw[XYZ_AXIS_COUNT];
@ -49,9 +50,18 @@ static int16_t accADCRaw[XYZ_AXIS_COUNT];
static flightDynamicsTrims_t * accZero;
static flightDynamicsTrims_t * accGain;
static int8_t accLpfCutHz = 0;
static biquadFilter_t accFilterState[XYZ_AXIS_COUNT];
static bool accFilterInitialised = false;
static uint8_t accLpfCutHz = 0;
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
void accInit(uint32_t targetLooptime)
{
accTargetLooptime = targetLooptime;
if (accLpfCutHz) {
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
}
}
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
@ -166,7 +176,7 @@ void performAcclerationCalibration(void)
calibratingA--;
}
void applyAccelerationZero(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)
{
accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
@ -179,23 +189,13 @@ void updateAccelerationReadings(void)
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = accADCRaw[axis];
}
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&accFilterState[axis], accLpfCutHz, 0);
}
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = lrintf(biquadFilterApply(&accFilterState[axis], (float) accADC[axis]));
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float) accADC[axis]));
}
}
@ -218,7 +218,12 @@ void setAccelerationGain(flightDynamicsTrims_t * accGainToUse)
accGain = accGainToUse;
}
void setAccelerationFilter(int8_t initialAccLpfCutHz)
void setAccelerationFilter(uint8_t initialAccLpfCutHz)
{
accLpfCutHz = initialAccLpfCutHz;
if (accTargetLooptime) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
}
}