From 8e16d83a379eb49af0c5b4cca28d15452eb8b63e Mon Sep 17 00:00:00 2001 From: "timecop@gmail.com" Date: Mon, 10 Sep 2012 09:11:22 +0000 Subject: [PATCH] testing: merged patch from http://www.multiwii.com/forum/viewtopic.php?p=22485#p22485 no binaries, as it doesn't work at the claimed PIDs of 5.0-0.030-30 git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@214 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61 --- src/config.c | 3 +- src/imu.c | 123 ++++++++++++++++++++++++++++++++++++++++++++++++++- src/mw.h | 1 + 3 files changed, 125 insertions(+), 2 deletions(-) diff --git a/src/config.c b/src/config.c index d922db697..a2f048e46 100755 --- a/src/config.c +++ b/src/config.c @@ -13,7 +13,7 @@ config_t cfg; const char rcChannelLetters[] = "AERT1234"; static uint32_t enabledSensors = 0; -uint8_t checkNewConf = 29; +uint8_t checkNewConf = 30; void parseRcChannels(const char *input) { @@ -135,6 +135,7 @@ void checkFirstTime(bool reset) cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. cfg.acc_hardware = ACC_DEFAULT; // default/autodetect cfg.acc_lpf_factor = 4; + cfg.acc_lpf_for_velocity = 10; cfg.gyro_cmpf_factor = 400; // default MWC cfg.gyro_lpf = 42; cfg.mpu6050_scale = 1; // fuck invensense diff --git a/src/imu.c b/src/imu.c index c2d55c854..889d4a04c 100755 --- a/src/imu.c +++ b/src/imu.c @@ -10,6 +10,7 @@ int16_t BaroPID = 0; int32_t AltHold; int16_t errorAltitudeI = 0; float magneticDeclination = 0.0f; // calculated at startup from config +float accVelScale; // ************** // gyro+acc IMU @@ -23,6 +24,7 @@ static void getEstimatedAttitude(void); void imuInit(void) { acc_25deg = acc_1G * 0.423f; + accVelScale = 9.80665f / acc_1G / 10000.0f; #ifdef MAG // if mag sensor is enabled, use it @@ -172,11 +174,13 @@ static int16_t _atan2f(float y, float x) return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f)); } +static t_fp_vector EstG; +static float accLPFVel[3]; + static void getEstimatedAttitude(void) { uint32_t axis; int32_t accMag = 0; - static t_fp_vector EstG; static t_fp_vector EstM; #if defined(MG_LPF_FACTOR) static int16_t mgSmooth[3]; @@ -198,6 +202,7 @@ static void getEstimatedAttitude(void) } else { accSmooth[axis] = accADC[axis]; } + accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f / cfg.acc_lpf_for_velocity)) + accADC[axis] * (1.0f / cfg.acc_lpf_for_velocity); accMag += (int32_t)accSmooth[axis] * accSmooth[axis]; if (sensors(SENSOR_MAG)) { @@ -253,10 +258,125 @@ static void getEstimatedAttitude(void) } #ifdef BARO +// #define NEW_ACCZ_HOLD #define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) #define INIT_DELAY 4000000 // 4 sec initialization delay #define BARO_TAB_SIZE 40 +#define ACC_Z_DEADBAND (acc_1G / 50) +#ifdef NEW_ACCZ_HOLD +int16_t applyDeadband16(int16_t value, int16_t deadband) +{ + if (abs(value) < deadband) { + value = 0; + } else if (value > 0) { + value -= deadband; + } else if (value < 0) { + value += deadband; + } + return value; +} + +float applyDeadbandFloat(float value, int16_t deadband) +{ + if (abs(value) < deadband) { + value = 0; + } else if (value > 0) { + value -= deadband; + } else if (value < 0) { + value += deadband; + } + return value; +} + +float InvSqrt (float x) +{ + union { + int32_t i; + float f; + } conv; + conv.f = x; + conv.i = 0x5f3759df - (conv.i >> 1); + return 0.5f * conv.f * (3.0f - x * conv.f * conv.f); +} + +int32_t isq(int32_t x) +{ + return x * x; +} + +void getEstimatedAltitude(void) +{ + uint8_t index; + static uint32_t deadLine = INIT_DELAY; + static int16_t BaroHistTab[BARO_TAB_SIZE]; + static int8_t BaroHistIdx; + static int32_t BaroHigh; + uint16_t dTime; + int16_t error; + float invG; + int16_t accZ; + static float vel = 0.0f; + static int32_t lastBaroAlt; + float baroVel; + + if ((int32_t)(currentTime - deadLine) < UPDATE_INTERVAL) + return; + dTime = currentTime - deadLine; + deadLine = currentTime; + + //**** Alt. Set Point stabilization PID **** + BaroHistTab[BaroHistIdx] = BaroAlt / 10; + BaroHigh += BaroHistTab[BaroHistIdx]; + index = (BaroHistIdx + (BARO_TAB_SIZE / 2)) % BARO_TAB_SIZE; + BaroHigh -= BaroHistTab[index]; + + BaroHistIdx++; + if (BaroHistIdx == BARO_TAB_SIZE) + BaroHistIdx = 0; + + // EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2); + EstAlt = EstAlt * 0.6f + (BaroHigh * 10.0f / (BARO_TAB_SIZE / 2)) * 0.4f; // additional LPF to reduce baro noise + + // P + error = applyDeadband16(AltHold - EstAlt, 10); // remove small P parametr to reduce noise near zero position + BaroPID = constrain((cfg.P8[PIDALT] * error / 100), -100, +100); + + // I + errorAltitudeI += error * cfg.I8[PIDALT] / 50; + errorAltitudeI = constrain(errorAltitudeI, -30000, 30000); + BaroPID += (errorAltitudeI / 500); // I in range +/-60 + + // projection of ACC vector to global Z, with 1G subtructed + // Math: accZ = A * G / |G| - 1G + invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z)); + // int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - acc_1G; + // int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - 1/invG; + accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G; + accZ = applyDeadband16(accZ, ACC_Z_DEADBAND); + debug[0] = accZ; + + // Integrator - velocity, cm/sec + vel += accZ * accVelScale * dTime; + + lastBaroAlt = EstAlt; + baroVel = (EstAlt - lastBaroAlt) / (dTime / 1000000.0f); + baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s + baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero + lastBaroAlt = EstAlt; + debug[1] = baroVel; + + // apply Complimentary Filter to keep near zero caluculated velocity based on baro velocity + vel = vel * 0.985f + baroVel * 0.015f; + // vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s + debug[2] = vel; + // debug[3] = applyDeadbandFloat(vel, 5); + + // D + BaroPID -= constrain(cfg.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150); + debug[3] = BaroPID; +} +#else void getEstimatedAltitude(void) { uint32_t index; @@ -305,4 +425,5 @@ void getEstimatedAltitude(void) temp32 = errorAltitudeI / 500; // I in range +/-60 BaroPID += temp32; } +#endif #endif /* BARO */ diff --git a/src/mw.h b/src/mw.h index 09e0dbe15..280ea167d 100755 --- a/src/mw.h +++ b/src/mw.h @@ -139,6 +139,7 @@ typedef struct config_t { // sensor-related stuff uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device 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 + uint8_t acc_lpf_for_velocity; // ACC lowpass for AccZ height hold uint16_t gyro_lpf; // mpuX050 LPF setting uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter. uint32_t gyro_smoothing_factor; // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively