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

Improved acc initialisation

Browse source
This commit is contained in:
Martin Budden 2016-08-15 07:32:06 +01:00
parent 9b565384f8
commit c077bacee6
7 changed files with 82 additions and 65 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -41,7 +41,7 @@ int32_t accSmooth[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions
sensor_align_e accAlign = 0;
uint32_t accTargetLooptime;
uint32_t accSamplingInterval;
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.
@ -52,9 +52,33 @@ extern bool AccInflightCalibrationActive;
static flightDynamicsTrims_t *accelerationTrims;
static float accLpfCutHz = 0;
static uint16_t accLpfCutHz = 0;
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
static bool accFilterInitialised = false;
void accInit(uint32_t gyroSamplingInverval)
{
// set the acc sampling interval according to the gyro sampling interval
switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future
case 500:
case 375:
case 250:
case 125:
accSamplingInterval = 1000;
break;
case 1000:
default:
#ifdef STM32F10X
accSamplingInterval = 1000;
#else
accSamplingInterval = 1000;
#endif
}
if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval);
}
}
}
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
@ -188,19 +212,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
}
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
}
}
@ -222,7 +235,12 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
accelerationTrims = accelerationTrimsToUse;
}
void setAccelerationFilter(float initialAccLpfCutHz)
void setAccelerationFilter(uint16_t initialAccLpfCutHz)
{
accLpfCutHz = initialAccLpfCutHz;
if (accSamplingInterval) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval);
}
}
}