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

Gyro native rate sampling, filtering, and scheduler restructuring

Browse source
This commit is contained in:
Bruce Luckcuck 2020-01-06 07:52:35 -05:00
parent b11565efbe
commit f584780944
42 changed files with 718 additions and 421 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -323,7 +323,20 @@ retry:
return true;
}
bool accInit(uint32_t gyroSamplingInverval)
void accInitFilters(void)
{
// Only set the lowpass cutoff if the ACC sample rate is detected otherwise
// the filter initialization is not defined (sample rate = 0)
accLpfCutHz = (acc.sampleRateHz) ? accelerometerConfig()->acc_lpf_hz : 0;
if (accLpfCutHz) {
const uint32_t accSampleTimeUs = 1e6 / acc.sampleRateHz;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSampleTimeUs);
}
}
}
bool accInit(uint16_t accSampleRateHz)
{
memset(&acc, 0, sizeof(acc));
// copy over the common gyro mpu settings
@ -353,23 +366,9 @@ bool accInit(uint32_t gyroSamplingInverval)
acc.dev.acc_1G = 256; // set default
acc.dev.initFn(&acc.dev); // driver initialisation
acc.dev.acc_1G_rec = 1.0f / acc.dev.acc_1G;
// 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:
acc.accSamplingInterval = 1000;
break;
case 1000:
default:
acc.accSamplingInterval = 1000;
}
if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
}
}
acc.sampleRateHz = accSampleRateHz;
accInitFilters();
return true;
}
@ -552,16 +551,6 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
accelerationTrims = accelerationTrimsToUse;
}
void accInitFilters(void)
{
accLpfCutHz = accelerometerConfig()->acc_lpf_hz;
if (acc.accSamplingInterval) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
}
}
}
void applyAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;