mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 21:05:35 +03:00
Rework Acc filtering
This commit is contained in:
borisbstyle
parent
974274b748
commit
57a3e59a38
8 changed files with 48 additions and 46 deletions
|
|
@ -492,8 +492,7 @@ static void resetConf(void)
|
|||
resetRollAndPitchTrims(&masterConfig.accelerometerTrims);
|
||||
|
||||
masterConfig.mag_declination = 0;
|
||||
masterConfig.acc_lpf_hz = 20;
|
||||
masterConfig.accz_lpf_cutoff = 5.0f;
|
||||
masterConfig.acc_lpf_hz = 10.0f;
|
||||
masterConfig.accDeadband.xy = 40;
|
||||
masterConfig.accDeadband.z = 40;
|
||||
masterConfig.acc_unarmedcal = 1;
|
||||
|
|
@ -730,6 +729,7 @@ void activateConfig(void)
|
|||
|
||||
useFailsafeConfig(&masterConfig.failsafeConfig);
|
||||
setAccelerationTrims(&masterConfig.accZero);
|
||||
setAccelerationFilter(masterConfig.acc_lpf_hz);
|
||||
|
||||
mixerUseConfigs(
|
||||
#ifdef USE_SERVOS
|
||||
|
|
@ -745,7 +745,6 @@ void activateConfig(void)
|
|||
|
||||
imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
|
||||
imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f;
|
||||
imuRuntimeConfig.acc_cut_hz = masterConfig.acc_lpf_hz;
|
||||
imuRuntimeConfig.acc_unarmedcal = masterConfig.acc_unarmedcal;
|
||||
imuRuntimeConfig.small_angle = masterConfig.small_angle;
|
||||
|
||||
|
|
@ -753,7 +752,6 @@ void activateConfig(void)
|
|||
&imuRuntimeConfig,
|
||||
¤tProfile->pidProfile,
|
||||
&masterConfig.accDeadband,
|
||||
masterConfig.accz_lpf_cutoff,
|
||||
masterConfig.throttle_correction_angle
|
||||
);
|
||||
|
||||
|
|
|
|||
|
|
@ -72,8 +72,7 @@ typedef struct master_t {
|
|||
|
||||
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
|
||||
|
||||
uint8_t acc_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
|
||||
float accz_lpf_cutoff; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
|
||||
float acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
|
||||
accDeadband_t accDeadband;
|
||||
barometerConfig_t barometerConfig;
|
||||
uint8_t acc_unarmedcal; // turn automatic acc compensation on/off
|
||||
|
|
|
|||
|
|
@ -56,7 +56,6 @@
|
|||
// http://gentlenav.googlecode.com/files/fastRotations.pdf
|
||||
#define SPIN_RATE_LIMIT 20
|
||||
|
||||
int16_t accSmooth[XYZ_AXIS_COUNT];
|
||||
int32_t accSum[XYZ_AXIS_COUNT];
|
||||
|
||||
uint32_t accTimeSum = 0; // keep track for integration of acc
|
||||
|
|
@ -111,14 +110,13 @@ void imuConfigure(
|
|||
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
||||
pidProfile_t *initialPidProfile,
|
||||
accDeadband_t *initialAccDeadband,
|
||||
float accz_lpf_cutoff,
|
||||
uint16_t throttle_correction_angle
|
||||
)
|
||||
{
|
||||
imuRuntimeConfig = initialImuRuntimeConfig;
|
||||
pidProfile = initialPidProfile;
|
||||
accDeadband = initialAccDeadband;
|
||||
fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
|
||||
fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
|
||||
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
|
||||
}
|
||||
|
||||
|
|
@ -377,11 +375,8 @@ static bool isMagnetometerHealthy(void)
|
|||
|
||||
static void imuCalculateEstimatedAttitude(void)
|
||||
{
|
||||
static biquad_t accLPFState[3];
|
||||
static bool accStateIsSet;
|
||||
static uint32_t previousIMUUpdateTime;
|
||||
float rawYawError = 0;
|
||||
int32_t axis;
|
||||
bool useAcc = false;
|
||||
bool useMag = false;
|
||||
bool useYaw = false;
|
||||
|
|
@ -390,20 +385,6 @@ static void imuCalculateEstimatedAttitude(void)
|
|||
uint32_t deltaT = currentTime - previousIMUUpdateTime;
|
||||
previousIMUUpdateTime = currentTime;
|
||||
|
||||
// Smooth and use only valid accelerometer readings
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
if (imuRuntimeConfig->acc_cut_hz) {
|
||||
if (accStateIsSet) {
|
||||
accSmooth[axis] = lrintf(applyBiQuadFilter((float) accADC[axis], &accLPFState[axis]));
|
||||
} else {
|
||||
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(imuRuntimeConfig->acc_cut_hz, &accLPFState[axis], 1000);
|
||||
accStateIsSet = true;
|
||||
}
|
||||
} else {
|
||||
accSmooth[axis] = accADC[axis];
|
||||
}
|
||||
}
|
||||
|
||||
if (imuIsAccelerometerHealthy()) {
|
||||
useAcc = true;
|
||||
}
|
||||
|
|
@ -445,9 +426,9 @@ void imuUpdateGyroAndAttitude(void)
|
|||
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
|
||||
imuCalculateEstimatedAttitude();
|
||||
} else {
|
||||
accADC[X] = 0;
|
||||
accADC[Y] = 0;
|
||||
accADC[Z] = 0;
|
||||
accSmooth[X] = 0;
|
||||
accSmooth[Y] = 0;
|
||||
accSmooth[Z] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -21,7 +21,6 @@ extern int16_t throttleAngleCorrection;
|
|||
extern uint32_t accTimeSum;
|
||||
extern int accSumCount;
|
||||
extern float accVelScale;
|
||||
extern int16_t accSmooth[XYZ_AXIS_COUNT];
|
||||
extern int32_t accSum[XYZ_AXIS_COUNT];
|
||||
|
||||
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
|
||||
|
|
@ -74,7 +73,6 @@ void imuConfigure(
|
|||
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
||||
pidProfile_t *initialPidProfile,
|
||||
accDeadband_t *initialAccDeadband,
|
||||
float accz_lpf_cutoff,
|
||||
uint16_t throttle_correction_angle
|
||||
);
|
||||
|
||||
|
|
@ -84,7 +82,7 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims);
|
|||
void imuUpdateGyroAndAttitude(void);
|
||||
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
|
||||
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
||||
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
|
||||
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
|
||||
|
||||
int16_t imuCalculateHeading(t_fp_vector *vec);
|
||||
|
||||
|
|
|
|||
|
|
@ -658,10 +658,9 @@ const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
|
||||
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
|
||||
{ "acc_lpf_hz", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 200 } },
|
||||
{ "acc_lpf_hz", VAR_FLOAT | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 400 } },
|
||||
{ "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0, 100 } },
|
||||
{ "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0, 100 } },
|
||||
{ "accz_lpf_cutoff", VAR_FLOAT | MASTER_VALUE, &masterConfig.accz_lpf_cutoff, .config.minmax = { 1, 20 } },
|
||||
{ "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } },
|
||||
{ "acc_trim_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300, 300 } },
|
||||
|
|
|
|||
|
|
@ -651,7 +651,6 @@ int main(void) {
|
|||
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
if(sensors(SENSOR_ACC)) {
|
||||
uint32_t accTargetLooptime = 0;
|
||||
setTaskEnabled(TASK_ACCEL, true);
|
||||
switch(targetLooptime) {
|
||||
case(500):
|
||||
|
|
|
|||
|
|
@ -17,14 +17,17 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
|
@ -35,11 +38,12 @@
|
|||
|
||||
#include "sensors/acceleration.h"
|
||||
|
||||
int32_t accADC[XYZ_AXIS_COUNT];
|
||||
int32_t accSmooth[XYZ_AXIS_COUNT];
|
||||
|
||||
acc_t acc; // acc access functions
|
||||
sensor_align_e accAlign = 0;
|
||||
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
|
||||
uint32_t accTargetLooptime;
|
||||
|
||||
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.
|
||||
|
||||
|
|
@ -51,6 +55,10 @@ extern bool AccInflightCalibrationActive;
|
|||
|
||||
static flightDynamicsTrims_t *accelerationTrims;
|
||||
|
||||
static float accLpfCutHz = 0;
|
||||
static biquad_t accFilterState[XYZ_AXIS_COUNT];
|
||||
static bool accFilterInitialised = false;
|
||||
|
||||
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||
{
|
||||
calibratingA = calibrationCyclesRequired;
|
||||
|
|
@ -89,10 +97,10 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
|
|||
a[axis] = 0;
|
||||
|
||||
// Sum up CALIBRATING_ACC_CYCLES readings
|
||||
a[axis] += accADC[axis];
|
||||
a[axis] += accSmooth[axis];
|
||||
|
||||
// Reset global variables to prevent other code from using un-calibrated data
|
||||
accADC[axis] = 0;
|
||||
accSmooth[axis] = 0;
|
||||
accelerationTrims->raw[axis] = 0;
|
||||
}
|
||||
|
||||
|
|
@ -131,9 +139,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
|
|||
if (InflightcalibratingA == 50)
|
||||
b[axis] = 0;
|
||||
// Sum up 50 readings
|
||||
b[axis] += accADC[axis];
|
||||
b[axis] += accSmooth[axis];
|
||||
// Clear global variables for next reading
|
||||
accADC[axis] = 0;
|
||||
accSmooth[axis] = 0;
|
||||
accelerationTrims->raw[axis] = 0;
|
||||
}
|
||||
// all values are measured
|
||||
|
|
@ -165,9 +173,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
|
|||
|
||||
void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
|
||||
{
|
||||
accADC[X] -= accelerationTrims->raw[X];
|
||||
accADC[Y] -= accelerationTrims->raw[Y];
|
||||
accADC[Z] -= accelerationTrims->raw[Z];
|
||||
accSmooth[X] -= accelerationTrims->raw[X];
|
||||
accSmooth[Y] -= accelerationTrims->raw[Y];
|
||||
accSmooth[Z] -= accelerationTrims->raw[Z];
|
||||
}
|
||||
|
||||
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
|
||||
|
|
@ -179,9 +187,22 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
|
|||
return;
|
||||
}
|
||||
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = accADCRaw[axis];
|
||||
|
||||
alignSensors(accADC, accADC, accAlign);
|
||||
if (accLpfCutHz) {
|
||||
if (!accFilterInitialised) {
|
||||
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
|
||||
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(accLpfCutHz, &accFilterState[axis], accTargetLooptime);
|
||||
accFilterInitialised = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (accFilterInitialised) {
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = lrintf(applyBiQuadFilter((float) accSmooth[axis], &accFilterState[axis]));
|
||||
}
|
||||
}
|
||||
|
||||
alignSensors(accSmooth, accSmooth, accAlign);
|
||||
|
||||
if (!isAccelerationCalibrationComplete()) {
|
||||
performAcclerationCalibration(rollAndPitchTrims);
|
||||
|
|
@ -198,3 +219,8 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
|
|||
{
|
||||
accelerationTrims = accelerationTrimsToUse;
|
||||
}
|
||||
|
||||
void setAccelerationFilter(float initialAccLpfCutHz)
|
||||
{
|
||||
accLpfCutHz = initialAccLpfCutHz;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -36,8 +36,9 @@ typedef enum {
|
|||
extern sensor_align_e accAlign;
|
||||
extern acc_t acc;
|
||||
extern uint16_t acc_1G;
|
||||
extern uint32_t accTargetLooptime;
|
||||
|
||||
extern int32_t accADC[XYZ_AXIS_COUNT];
|
||||
extern int32_t accSmooth[XYZ_AXIS_COUNT];
|
||||
|
||||
typedef struct rollAndPitchTrims_s {
|
||||
int16_t roll;
|
||||
|
|
@ -54,3 +55,4 @@ void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
|||
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
|
||||
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
|
||||
void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse);
|
||||
void setAccelerationFilter(float initialAccLpfCutHz);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue