improved altitude hold thanks to Luggi09
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@386 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com
parent
659a8f537f
commit
f663a57613
|
|
@ -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 },
|
||||
|
|
|
|||
13
src/config.c
13
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");
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
138
src/imu.c
138
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);
|
||||
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
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
|
|
|||
10
src/mw.h
10
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;
|
||||
|
|
|
|||
|
|
@ -439,8 +439,14 @@ static void evaluateCommand(void)
|
|||
break;
|
||||
case MSP_RAW_IMU:
|
||||
headSerialReply(18);
|
||||
// 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++)
|
||||
|
|
|
|||
Loading…
Reference in New Issue