diff --git a/src/cli.c b/src/cli.c index c12371af94..4b689bf447 100644 --- a/src/cli.c +++ b/src/cli.c @@ -175,11 +175,15 @@ const clivalue_t valueTable[] = { { "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 }, { "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 }, { "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 }, + { "accxy_deadband", VAR_UINT8, &cfg.accxy_deadband, 0, 100 }, + { "accz_deadband", VAR_UINT8, &cfg.accz_deadband, 0, 100 }, + { "acc_unarmedcal", VAR_UINT8, &cfg.acc_unarmedcal, 0, 1 }, { "acc_trim_pitch", VAR_INT16, &cfg.angleTrim[PITCH], -300, 300 }, { "acc_trim_roll", VAR_INT16, &cfg.angleTrim[ROLL], -300, 300 }, { "baro_tab_size", VAR_UINT8, &cfg.baro_tab_size, 0, BARO_TAB_SIZE_MAX }, { "baro_noise_lpf", VAR_FLOAT, &cfg.baro_noise_lpf, 0, 1 }, - { "baro_cf", VAR_FLOAT, &cfg.baro_cf, 0, 1 }, + { "baro_cf_vel", VAR_FLOAT, &cfg.baro_cf_vel, 0, 1 }, + { "baro_cf_alt", VAR_FLOAT, &cfg.baro_cf_alt, 0, 1 }, { "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 }, { "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 }, { "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 }, diff --git a/src/config.c b/src/config.c index 9e88f6f301..2198a72cdb 100755 --- a/src/config.c +++ b/src/config.c @@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; -static const uint8_t EEPROM_CONF_VERSION = 48; +static const uint8_t EEPROM_CONF_VERSION = 49; static uint32_t enabledSensors = 0; static void resetConf(void); @@ -217,9 +217,9 @@ static void resetConf(void) cfg.P8[YAW] = 85; cfg.I8[YAW] = 45; cfg.D8[YAW] = 0; - cfg.P8[PIDALT] = 64; + cfg.P8[PIDALT] = 50; cfg.I8[PIDALT] = 25; - cfg.D8[PIDALT] = 24; + cfg.D8[PIDALT] = 80; cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100; cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100; cfg.D8[PIDPOS] = 0; @@ -249,10 +249,13 @@ static void resetConf(void) cfg.angleTrim[1] = 0; cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. cfg.acc_lpf_factor = 4; - cfg.accz_deadband = 50; + cfg.accz_deadband = 40; + cfg.accxy_deadband = 40; cfg.baro_tab_size = 21; cfg.baro_noise_lpf = 0.6f; - cfg.baro_cf = 0.985f; + cfg.baro_cf_vel = 0.995f; + cfg.baro_cf_alt = 0.900f; + cfg.acc_unarmedcal = 1; // Radio parseRcChannels("AETR1234"); diff --git a/src/drv_mpu6050.c b/src/drv_mpu6050.c index 5323ab72f3..7dc6e9eff3 100644 --- a/src/drv_mpu6050.c +++ b/src/drv_mpu6050.c @@ -233,9 +233,9 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale) static void mpu6050AccInit(void) { if (mpuAccelHalf) - acc_1G = 255; + acc_1G = 255 * 8; else - acc_1G = 512; + acc_1G = 512 * 8; } static void mpu6050AccRead(int16_t *accData) @@ -244,9 +244,9 @@ static void mpu6050AccRead(int16_t *accData) #ifndef MPU6050_DMP i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf); - accData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 8; - accData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 8; - accData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 8; + accData[0] = (int16_t)((buf[0] << 8) | buf[1]); + accData[1] = (int16_t)((buf[2] << 8) | buf[3]); + accData[2] = (int16_t)((buf[4] << 8) | buf[5]); #else accData[0] = accData[1] = accData[2] = 0; #endif diff --git a/src/imu.c b/src/imu.c index 8470b4f26d..5adf86fb31 100755 --- a/src/imu.c +++ b/src/imu.c @@ -2,6 +2,9 @@ #include "mw.h" int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; +int32_t accSum[3]; +uint32_t accTimeSum = 0; // keep track for integration of acc +int accSumCount = 0; int16_t acc_25deg = 0; int32_t baroPressure = 0; int32_t baroTemperature = 0; @@ -178,6 +181,68 @@ static int16_t _atan2f(float y, float x) return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f)); } +int32_t applyDeadband(int32_t value, int32_t deadband) +{ + if (abs(value) < deadband) { + value = 0; + } else if (value > 0) { + value -= deadband; + } else if (value < 0) { + value += deadband; + } + return value; +} + +// rotate acc into Earth frame and calculate acceleration in it +void acc_calc(uint32_t deltaT) +{ + static int32_t accZoffset = 0; + float rpy[3]; + t_fp_vector accel_ned; + + // the accel values have to be rotated into the earth frame + rpy[0] = -(float) angle[ROLL] * RADX10; + rpy[1] = -(float) angle[PITCH] * RADX10; + rpy[2] = -(float) heading * RADX10 * 10; + + accel_ned.V.X = accSmooth[0]; + accel_ned.V.Y = accSmooth[1]; + accel_ned.V.Z = accSmooth[2]; + + rotateV(&accel_ned.V, rpy); + + if (cfg.acc_unarmedcal == 1) { + if (!f.ARMED) { + accZoffset -= accZoffset / 64; + accZoffset += accel_ned.V.Z; + } + accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis + } else + accel_ned.V.Z -= acc_1G; + + // apply Deadband to reduce integration drift and vibration influence + accel_ned.V.Z = applyDeadband(accel_ned.V.Z, cfg.accz_deadband); + accel_ned.V.X = applyDeadband(accel_ned.V.X, cfg.accxy_deadband); + accel_ned.V.Y = applyDeadband(accel_ned.V.Y, cfg.accxy_deadband); + + // sum up Values for later integration to get velocity and distance + accTimeSum += deltaT; + accSumCount++; + + accSum[0] += accel_ned.V.X; + accSum[1] += accel_ned.V.Y; + accSum[2] += accel_ned.V.Z; +} + +void accSum_reset(void) +{ + accSum[0] = 0; + accSum[1] = 0; + accSum[2] = 0; + accSumCount = 0; + accTimeSum = 0; +} + // Use original baseflight angle calculation // #define BASEFLIGHT_CALC static float invG; @@ -190,6 +255,7 @@ static void getEstimatedAttitude(void) static float accLPF[3]; static uint32_t previousT; uint32_t currentT = micros(); + uint32_t deltaT; float scale, deltaGyroAngle[3]; #ifndef BASEFLIGHT_CALC float sqGZ; @@ -198,8 +264,8 @@ static void getEstimatedAttitude(void) float sqGX_sqGZ; float invmagXZ; #endif - - scale = (currentT - previousT) * gyro.scale; + deltaT = currentT - previousT; + scale = deltaT * gyro.scale; previousT = currentT; // Initialization @@ -284,6 +350,8 @@ static void getEstimatedAttitude(void) } #endif + acc_calc(deltaT); // rotate acc vector into earth frame + if (cfg.throttle_angle_correction) { int cosZ = EstG.V.Z / acc_1G * 100.0f; throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8; @@ -293,19 +361,6 @@ static void getEstimatedAttitude(void) #ifdef BARO #define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) -#define INIT_DELAY 4000000 // 4 sec initialization delay - -int16_t applyDeadband(int32_t value, int32_t deadband) -{ - if (abs(value) < deadband) { - value = 0; - } else if (value > 0) { - value -= deadband; - } else if (value < 0) { - value += deadband; - } - return value; -} int getEstimatedAltitude(void) { @@ -315,10 +370,11 @@ int getEstimatedAltitude(void) uint32_t dTime; int32_t error; int32_t baroVel; - int32_t accZ; int32_t vel_tmp; - static int32_t accZoffset = 0; + int32_t BaroAlt_tmp; + float vel_calc; static float vel = 0.0f; + static float accAlt = 0.0f; static int32_t lastBaroAlt; dTime = currentT - previousT; @@ -329,53 +385,59 @@ int getEstimatedAltitude(void) if (calibratingB > 0) { baroGroundPressure = baroPressureSum / (cfg.baro_tab_size - 1); calibratingB--; + vel = 0; + accAlt = 0; } // pressure relative to ground pressure with temperature compensation (fast!) // baroGroundPressure is not supposed to be 0 here // see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp - BaroAlt = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter - EstAlt = (EstAlt * 6 + BaroAlt * 2) / 8; // additional LPF to reduce baro noise + BaroAlt_tmp = logf(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter + BaroAlt = BaroAlt * cfg.baro_noise_lpf + BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise + + // Integrator - velocity, cm/sec + vel_calc = (float) accSum[2] * accVelScale * (float) accTimeSum / (float) accSumCount; + vel += vel_calc; + + // Integrator - Altitude in cm + accAlt += vel * ((float) accTimeSum * 0.0000005f); // integrate velocity to get distance (x= a/2 * t^2) + accAlt = accAlt * cfg.baro_cf_alt + (float) BaroAlt *(1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc) + EstAlt = accAlt; + +#if 0 + debug[0] = accSum[2] / accSumCount; // acceleration + debug[1] = vel; // velocity + debug[2] = accAlt; // height +#endif + + accSum_reset(); //P error = constrain(AltHold - EstAlt, -300, 300); - error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position - BaroPID = constrain(((cfg.P8[PIDALT] * error) / 128), -150, +150); + error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position + BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -150, +150); //I - errorAltitudeI += (cfg.I8[PIDALT] * error) / 64; + errorAltitudeI += cfg.I8[PIDALT] * error / 64; errorAltitudeI = constrain(errorAltitudeI, -30000, 30000); BaroPID += errorAltitudeI / 512; // I in range +/-60 - // projection of ACC vector to global Z, with 1G subtructed - // Math: accZ = A * G / |G| - 1G (invG is calculated in getEstimatedAttitude) - accZ = (accSmooth[ROLL] * EstG.V.X + accSmooth[PITCH] * EstG.V.Y + accSmooth[YAW] * EstG.V.Z) * invG; - - if (!f.ARMED) { - accZoffset -= accZoffset / 8; - accZoffset += accZ; - } - accZ -= accZoffset / 8; - accZ = applyDeadband(accZ, cfg.accz_deadband); - - // Integrator - velocity, cm/sec - vel += accZ * accVelScale * dTime; - - baroVel = (EstAlt - lastBaroAlt) * 1000000.0f / dTime; - lastBaroAlt = EstAlt; + + baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime; + lastBaroAlt = BaroAlt; baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s - baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero + baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay - vel = vel * 0.985f + baroVel * 0.015f; + vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel); // D vel_tmp = vel; vel_tmp = applyDeadband(vel_tmp, 5); vario = vel_tmp; - BaroPID -= constrain((cfg.D8[PIDALT] * vel_tmp) / 16, -150, 150); + BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp / 16, -150, 150); return 1; } diff --git a/src/mw.h b/src/mw.h index 6d912b36cb..88103225f1 100755 --- a/src/mw.h +++ b/src/mw.h @@ -164,10 +164,13 @@ typedef struct config_t { // 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 - uint8_t accz_deadband; // ?? + uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations + uint8_t accxy_deadband; // set the acc deadband for xy-Axis uint8_t baro_tab_size; // size of baro filter array float baro_noise_lpf; // additional LPF to reduce baro noise - float baro_cf; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) + float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) + float baro_cf_alt; // apply CF to use ACC for height estimation + uint8_t acc_unarmedcal; // turn automatic acc compensation on/off uint16_t activate[CHECKBOXITEMS]; // activate switches @@ -325,7 +328,10 @@ extern int16_t failsafeCnt; extern int16_t debug[4]; extern int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; +extern int32_t accSum[3]; extern uint16_t acc_1G; +extern uint32_t accTimeSum; +extern int accSumCount; extern uint32_t currentTime; extern uint32_t previousTime; extern uint16_t cycleTime; diff --git a/src/serial.c b/src/serial.c index bf56a3c4b6..b35fe7a09f 100755 --- a/src/serial.c +++ b/src/serial.c @@ -439,8 +439,14 @@ static void evaluateCommand(void) break; case MSP_RAW_IMU: headSerialReply(18); - for (i = 0; i < 3; i++) - serialize16(accSmooth[i]); + // Retarded hack until multiwiidorks start using real units for sensor data + if (acc_1G > 1024) { + for (i = 0; i < 3; i++) + serialize16(accSmooth[i] / 8); + } else { + for (i = 0; i < 3; i++) + serialize16(accSmooth[i]); + } for (i = 0; i < 3; i++) serialize16(gyroData[i]); for (i = 0; i < 3; i++)