From 1bf806f54ce8b7c426edd3aa09edda2c6d63ce61 Mon Sep 17 00:00:00 2001 From: luggi Date: Tue, 3 Jun 2014 16:32:18 +0200 Subject: [PATCH] make the accZ lpf used for althold configurable set the variable accz_lpf_cutoff to the desired cutoff frequency this can help to make althold smoother on copters with lots of vibrations Conflicts: src/cli.c src/config.c src/imu.c src/mw.h --- src/main/config/config.c | 4 +++- src/main/config/config_profile.h | 3 ++- src/main/flight/imu.c | 9 ++++++--- src/main/flight/imu.h | 1 + src/main/io/serial_cli.c | 1 + 5 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ceb2aa720..7423dd654 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -85,7 +85,7 @@ static uint32_t flashWriteAddress = (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * master_t masterConfig; // master config struct with data independent from profiles profile_t currentProfile; // profile config struct -static const uint8_t EEPROM_CONF_VERSION = 73; +static const uint8_t EEPROM_CONF_VERSION = 74; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -305,6 +305,7 @@ static void resetConf(void) currentProfile.mag_declination = 0; currentProfile.acc_lpf_factor = 4; + currentProfile.accz_lpf_cutoff = 5.0f; currentProfile.accDeadband.xy = 40; currentProfile.accDeadband.z = 40; @@ -425,6 +426,7 @@ void activateConfig(void) configureImu(&imuRuntimeConfig, ¤tProfile.pidProfile, ¤tProfile.barometerConfig, ¤tProfile.accDeadband); calculateThrottleAngleScale(currentProfile.throttle_correction_angle); + calculateAccZLowPassFilterRCTimeConstant(currentProfile.accz_lpf_cutoff); #ifdef BARO useBarometerConfig(¤tProfile.barometerConfig); diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 192048be0..a4ac932ae 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -34,6 +34,7 @@ typedef struct profile_s { // sensor-related stuff uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing 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 accDeadband_t accDeadband; barometerConfig_t barometerConfig; @@ -46,7 +47,7 @@ typedef struct profile_s { // Radio/ESC-related configuration uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. - uint8_t alt_hold_deadband; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 + uint8_t alt_hold_deadband; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 20ea3250f..0d9efaf88 100755 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -61,6 +61,7 @@ int32_t errorAltitudeI = 0; int32_t vario = 0; // variometer in cm/s float throttleAngleScale; +float fc_acc; int32_t BaroPID = 0; @@ -103,6 +104,11 @@ void calculateThrottleAngleScale(uint16_t throttle_correction_angle) throttleAngleScale = (1800.0f / M_PI) * (900.0f / throttle_correction_angle); } +void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) +{ + fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf +} + void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration) { static int16_t gyroYawSmooth = 0; @@ -207,9 +213,6 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } -#define F_CUT_ACCZ 10.0f // 10Hz should still be fast enough -static const float fc_acc = 0.5f / (M_PI * F_CUT_ACCZ); - // rotate acc into Earth frame and calculate acceleration in it void acc_calc(uint32_t deltaT) { diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 78e2791c8..48a905808 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -35,3 +35,4 @@ void calculateEstimatedAltitude(uint32_t currentTime); void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration); void calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); +void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 19ab3a2a2..40134aa01 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -296,6 +296,7 @@ const clivalue_t valueTable[] = { { "acc_lpf_factor", VAR_UINT8, ¤tProfile.acc_lpf_factor, 0, 250 }, { "accxy_deadband", VAR_UINT8, ¤tProfile.accDeadband.xy, 0, 100 }, { "accz_deadband", VAR_UINT8, ¤tProfile.accDeadband.z, 0, 100 }, + { "accz_lpf_cutoff", VAR_FLOAT, ¤tProfile.accz_lpf_cutoff, 1, 20 }, { "acc_unarmedcal", VAR_UINT8, ¤tProfile.acc_unarmedcal, 0, 1 }, { "acc_trim_pitch", VAR_INT16, ¤tProfile.accelerometerTrims.values.pitch, -300, 300 }, { "acc_trim_roll", VAR_INT16, ¤tProfile.accelerometerTrims.values.roll, -300, 300 },