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
This commit is contained in:
timecop@gmail.com
parent
70884d69d5
commit
8e16d83a37
|
|
@ -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
|
||||
|
|
|
|||
123
src/imu.c
123
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 */
|
||||
|
|
|
|||
1
src/mw.h
1
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue