oops. gyro only does NOT need getEstimatedAttitude. bad!
ms5611 driver improvements (was failing below 20c) merged some althold cleanups git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@411 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
parent
1ff4bcec5a
commit
64e8f247bf
|
@ -154,29 +154,27 @@ static void ms5611_get_up(void)
|
|||
|
||||
static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
|
||||
{
|
||||
int32_t temp, off2 = 0, sens2 = 0, delt;
|
||||
int32_t press;
|
||||
|
||||
int64_t dT = ms5611_ut - ((int32_t)ms5611_c[5] << 8);
|
||||
int64_t off = ((uint32_t)ms5611_c[2] << 16) + ((dT * ms5611_c[4]) >> 7);
|
||||
int64_t sens = ((uint32_t)ms5611_c[1] << 15) + ((dT * ms5611_c[3]) >> 8);
|
||||
temp = 2000 + ((dT * ms5611_c[6]) >> 23);
|
||||
uint32_t press;
|
||||
int64_t temp;
|
||||
int64_t delt;
|
||||
int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
|
||||
int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
|
||||
int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
|
||||
temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
|
||||
|
||||
if (temp < 2000) { // temperature lower than 20degC
|
||||
delt = temp - 2000;
|
||||
delt = 5 * delt * delt;
|
||||
off2 = delt >> 1;
|
||||
sens2 = delt >> 2;
|
||||
off -= delt >> 1;
|
||||
sens -= delt >> 2;
|
||||
if (temp < -1500) { // temperature lower than -15degC
|
||||
delt = temp + 1500;
|
||||
delt = delt * delt;
|
||||
off2 += 7 * delt;
|
||||
sens2 += (11 * delt) >> 1;
|
||||
off -= 7 * delt;
|
||||
sens -= (11 * delt) >> 1;
|
||||
}
|
||||
}
|
||||
off -= off2;
|
||||
sens -= sens2;
|
||||
press = (((ms5611_up * sens ) >> 21) - off) >> 15;
|
||||
press = ((((int64_t)ms5611_up * sens ) >> 21) - off) >> 15;
|
||||
|
||||
if (pressure)
|
||||
*pressure = press;
|
||||
|
|
12
src/imu.c
12
src/imu.c
|
@ -8,7 +8,7 @@ int accSumCount = 0;
|
|||
int16_t acc_25deg = 0;
|
||||
int32_t baroPressure = 0;
|
||||
int32_t baroTemperature = 0;
|
||||
int32_t baroPressureSum = 0;
|
||||
uint32_t baroPressureSum = 0;
|
||||
int32_t BaroAlt = 0;
|
||||
int32_t sonarAlt; // to think about the unit
|
||||
int32_t EstAlt; // in cm
|
||||
|
@ -50,12 +50,12 @@ void computeIMU(void)
|
|||
Gyro_getADC();
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
ACC_getADC();
|
||||
getEstimatedAttitude();
|
||||
} else {
|
||||
accADC[X] = 0;
|
||||
accADC[Y] = 0;
|
||||
accADC[Z] = 0;
|
||||
}
|
||||
getEstimatedAttitude();
|
||||
|
||||
if (feature(FEATURE_GYRO_SMOOTHING)) {
|
||||
static uint8_t Smoothing[3] = { 0, 0, 0 };
|
||||
|
@ -367,13 +367,12 @@ int getEstimatedAltitude(void)
|
|||
//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), -200, +200);
|
||||
|
||||
//I
|
||||
errorAltitudeI += cfg.I8[PIDALT] * error / 64;
|
||||
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
|
||||
BaroPID += errorAltitudeI / 512; // I in range +/-60
|
||||
|
||||
errorAltitudeI = constrain(errorAltitudeI, -50000, 50000);
|
||||
BaroPID += errorAltitudeI / 512; // I in range +/-100
|
||||
|
||||
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
|
||||
lastBaroAlt = BaroAlt;
|
||||
|
@ -384,6 +383,7 @@ int getEstimatedAltitude(void)
|
|||
// 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 * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
|
||||
constrain(vel, -1000, 1000); // limit max velocity to +/- 10m/s (36km/h)
|
||||
|
||||
// D
|
||||
vel_tmp = vel;
|
||||
|
|
9
src/mw.c
9
src/mw.c
|
@ -823,17 +823,16 @@ void loop(void)
|
|||
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
|
||||
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
|
||||
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
|
||||
if (abs(AltHoldCorr) > 500) {
|
||||
AltHold += AltHoldCorr / 500;
|
||||
AltHoldCorr %= 500;
|
||||
}
|
||||
errorAltitudeI = 0;
|
||||
AltHold += AltHoldCorr / 2000;
|
||||
AltHoldCorr %= 2000;
|
||||
isAltHoldChanged = 1;
|
||||
} else if (isAltHoldChanged) {
|
||||
AltHold = EstAlt;
|
||||
AltHoldCorr = 0;
|
||||
isAltHoldChanged = 0;
|
||||
}
|
||||
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
|
||||
rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue