diff --git a/src/main/config/config.c b/src/main/config/config.c index c0fd1010ca..b77c2c9de1 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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 ); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index f74c418457..eb52cb4663 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -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 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 23141f85d7..29a8e4b04f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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; } } diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index c0b7d53fb8..c896484312 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b24799971d..917caf25ff 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 } }, diff --git a/src/main/main.c b/src/main/main.c index 5a71368cc0..d6f6380cc2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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): diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 175ba1bb75..45dd01f7e6 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -17,14 +17,17 @@ #include #include +#include #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; +} diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index b9644eccbb..d2d892fd6c 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -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);