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:
parent
70884d69d5
commit
8e16d83a37
|
@ -13,7 +13,7 @@ config_t cfg;
|
||||||
const char rcChannelLetters[] = "AERT1234";
|
const char rcChannelLetters[] = "AERT1234";
|
||||||
|
|
||||||
static uint32_t enabledSensors = 0;
|
static uint32_t enabledSensors = 0;
|
||||||
uint8_t checkNewConf = 29;
|
uint8_t checkNewConf = 30;
|
||||||
|
|
||||||
void parseRcChannels(const char *input)
|
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.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_hardware = ACC_DEFAULT; // default/autodetect
|
||||||
cfg.acc_lpf_factor = 4;
|
cfg.acc_lpf_factor = 4;
|
||||||
|
cfg.acc_lpf_for_velocity = 10;
|
||||||
cfg.gyro_cmpf_factor = 400; // default MWC
|
cfg.gyro_cmpf_factor = 400; // default MWC
|
||||||
cfg.gyro_lpf = 42;
|
cfg.gyro_lpf = 42;
|
||||||
cfg.mpu6050_scale = 1; // fuck invensense
|
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;
|
int32_t AltHold;
|
||||||
int16_t errorAltitudeI = 0;
|
int16_t errorAltitudeI = 0;
|
||||||
float magneticDeclination = 0.0f; // calculated at startup from config
|
float magneticDeclination = 0.0f; // calculated at startup from config
|
||||||
|
float accVelScale;
|
||||||
|
|
||||||
// **************
|
// **************
|
||||||
// gyro+acc IMU
|
// gyro+acc IMU
|
||||||
|
@ -23,6 +24,7 @@ static void getEstimatedAttitude(void);
|
||||||
void imuInit(void)
|
void imuInit(void)
|
||||||
{
|
{
|
||||||
acc_25deg = acc_1G * 0.423f;
|
acc_25deg = acc_1G * 0.423f;
|
||||||
|
accVelScale = 9.80665f / acc_1G / 10000.0f;
|
||||||
|
|
||||||
#ifdef MAG
|
#ifdef MAG
|
||||||
// if mag sensor is enabled, use it
|
// 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));
|
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)
|
static void getEstimatedAttitude(void)
|
||||||
{
|
{
|
||||||
uint32_t axis;
|
uint32_t axis;
|
||||||
int32_t accMag = 0;
|
int32_t accMag = 0;
|
||||||
static t_fp_vector EstG;
|
|
||||||
static t_fp_vector EstM;
|
static t_fp_vector EstM;
|
||||||
#if defined(MG_LPF_FACTOR)
|
#if defined(MG_LPF_FACTOR)
|
||||||
static int16_t mgSmooth[3];
|
static int16_t mgSmooth[3];
|
||||||
|
@ -198,6 +202,7 @@ static void getEstimatedAttitude(void)
|
||||||
} else {
|
} else {
|
||||||
accSmooth[axis] = accADC[axis];
|
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];
|
accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
|
||||||
|
|
||||||
if (sensors(SENSOR_MAG)) {
|
if (sensors(SENSOR_MAG)) {
|
||||||
|
@ -253,10 +258,125 @@ static void getEstimatedAttitude(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
|
// #define NEW_ACCZ_HOLD
|
||||||
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
|
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
|
||||||
#define INIT_DELAY 4000000 // 4 sec initialization delay
|
#define INIT_DELAY 4000000 // 4 sec initialization delay
|
||||||
#define BARO_TAB_SIZE 40
|
#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)
|
void getEstimatedAltitude(void)
|
||||||
{
|
{
|
||||||
uint32_t index;
|
uint32_t index;
|
||||||
|
@ -305,4 +425,5 @@ void getEstimatedAltitude(void)
|
||||||
temp32 = errorAltitudeI / 500; // I in range +/-60
|
temp32 = errorAltitudeI / 500; // I in range +/-60
|
||||||
BaroPID += temp32;
|
BaroPID += temp32;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
#endif /* BARO */
|
#endif /* BARO */
|
||||||
|
|
1
src/mw.h
1
src/mw.h
|
@ -139,6 +139,7 @@ typedef struct config_t {
|
||||||
// sensor-related stuff
|
// sensor-related stuff
|
||||||
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
|
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_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_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.
|
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
|
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