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)
|
static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
|
||||||
{
|
{
|
||||||
int32_t temp, off2 = 0, sens2 = 0, delt;
|
uint32_t press;
|
||||||
int32_t press;
|
int64_t temp;
|
||||||
|
int64_t delt;
|
||||||
int64_t dT = ms5611_ut - ((int32_t)ms5611_c[5] << 8);
|
int32_t dT = (int64_t)ms5611_ut - ((uint64_t)ms5611_c[5] * 256);
|
||||||
int64_t off = ((uint32_t)ms5611_c[2] << 16) + ((dT * ms5611_c[4]) >> 7);
|
int64_t off = ((int64_t)ms5611_c[2] << 16) + (((int64_t)ms5611_c[4] * dT) >> 7);
|
||||||
int64_t sens = ((uint32_t)ms5611_c[1] << 15) + ((dT * ms5611_c[3]) >> 8);
|
int64_t sens = ((int64_t)ms5611_c[1] << 15) + (((int64_t)ms5611_c[3] * dT) >> 8);
|
||||||
temp = 2000 + ((dT * ms5611_c[6]) >> 23);
|
temp = 2000 + ((dT * (int64_t)ms5611_c[6]) >> 23);
|
||||||
|
|
||||||
if (temp < 2000) { // temperature lower than 20degC
|
if (temp < 2000) { // temperature lower than 20degC
|
||||||
delt = temp - 2000;
|
delt = temp - 2000;
|
||||||
delt = 5 * delt * delt;
|
delt = 5 * delt * delt;
|
||||||
off2 = delt >> 1;
|
off -= delt >> 1;
|
||||||
sens2 = delt >> 2;
|
sens -= delt >> 2;
|
||||||
if (temp < -1500) { // temperature lower than -15degC
|
if (temp < -1500) { // temperature lower than -15degC
|
||||||
delt = temp + 1500;
|
delt = temp + 1500;
|
||||||
delt = delt * delt;
|
delt = delt * delt;
|
||||||
off2 += 7 * delt;
|
off -= 7 * delt;
|
||||||
sens2 += (11 * delt) >> 1;
|
sens -= (11 * delt) >> 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
off -= off2;
|
press = ((((int64_t)ms5611_up * sens ) >> 21) - off) >> 15;
|
||||||
sens -= sens2;
|
|
||||||
press = (((ms5611_up * sens ) >> 21) - off) >> 15;
|
|
||||||
|
|
||||||
if (pressure)
|
if (pressure)
|
||||||
*pressure = press;
|
*pressure = press;
|
||||||
|
|
12
src/imu.c
12
src/imu.c
|
@ -8,7 +8,7 @@ int accSumCount = 0;
|
||||||
int16_t acc_25deg = 0;
|
int16_t acc_25deg = 0;
|
||||||
int32_t baroPressure = 0;
|
int32_t baroPressure = 0;
|
||||||
int32_t baroTemperature = 0;
|
int32_t baroTemperature = 0;
|
||||||
int32_t baroPressureSum = 0;
|
uint32_t baroPressureSum = 0;
|
||||||
int32_t BaroAlt = 0;
|
int32_t BaroAlt = 0;
|
||||||
int32_t sonarAlt; // to think about the unit
|
int32_t sonarAlt; // to think about the unit
|
||||||
int32_t EstAlt; // in cm
|
int32_t EstAlt; // in cm
|
||||||
|
@ -50,12 +50,12 @@ void computeIMU(void)
|
||||||
Gyro_getADC();
|
Gyro_getADC();
|
||||||
if (sensors(SENSOR_ACC)) {
|
if (sensors(SENSOR_ACC)) {
|
||||||
ACC_getADC();
|
ACC_getADC();
|
||||||
|
getEstimatedAttitude();
|
||||||
} else {
|
} else {
|
||||||
accADC[X] = 0;
|
accADC[X] = 0;
|
||||||
accADC[Y] = 0;
|
accADC[Y] = 0;
|
||||||
accADC[Z] = 0;
|
accADC[Z] = 0;
|
||||||
}
|
}
|
||||||
getEstimatedAttitude();
|
|
||||||
|
|
||||||
if (feature(FEATURE_GYRO_SMOOTHING)) {
|
if (feature(FEATURE_GYRO_SMOOTHING)) {
|
||||||
static uint8_t Smoothing[3] = { 0, 0, 0 };
|
static uint8_t Smoothing[3] = { 0, 0, 0 };
|
||||||
|
@ -367,13 +367,12 @@ int getEstimatedAltitude(void)
|
||||||
//P
|
//P
|
||||||
error = constrain(AltHold - EstAlt, -300, 300);
|
error = constrain(AltHold - EstAlt, -300, 300);
|
||||||
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
|
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
|
//I
|
||||||
errorAltitudeI += cfg.I8[PIDALT] * error / 64;
|
errorAltitudeI += cfg.I8[PIDALT] * error / 64;
|
||||||
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
|
errorAltitudeI = constrain(errorAltitudeI, -50000, 50000);
|
||||||
BaroPID += errorAltitudeI / 512; // I in range +/-60
|
BaroPID += errorAltitudeI / 512; // I in range +/-100
|
||||||
|
|
||||||
|
|
||||||
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
|
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
|
||||||
lastBaroAlt = BaroAlt;
|
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).
|
// 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
|
// 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);
|
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
|
// D
|
||||||
vel_tmp = vel;
|
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) {
|
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)
|
// 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;
|
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
|
||||||
if (abs(AltHoldCorr) > 500) {
|
AltHold += AltHoldCorr / 2000;
|
||||||
AltHold += AltHoldCorr / 500;
|
AltHoldCorr %= 2000;
|
||||||
AltHoldCorr %= 500;
|
|
||||||
}
|
|
||||||
errorAltitudeI = 0;
|
|
||||||
isAltHoldChanged = 1;
|
isAltHoldChanged = 1;
|
||||||
} else if (isAltHoldChanged) {
|
} else if (isAltHoldChanged) {
|
||||||
AltHold = EstAlt;
|
AltHold = EstAlt;
|
||||||
|
AltHoldCorr = 0;
|
||||||
isAltHoldChanged = 0;
|
isAltHoldChanged = 0;
|
||||||
}
|
}
|
||||||
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
|
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
|
||||||
|
rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE], mcfg.minthrottle + 150, mcfg.maxthrottle);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
2
src/mw.h
2
src/mw.h
|
@ -345,7 +345,7 @@ extern uint16_t calibratingG;
|
||||||
extern int16_t heading;
|
extern int16_t heading;
|
||||||
extern int32_t baroPressure;
|
extern int32_t baroPressure;
|
||||||
extern int32_t baroTemperature;
|
extern int32_t baroTemperature;
|
||||||
extern int32_t baroPressureSum;
|
extern uint32_t baroPressureSum;
|
||||||
extern int32_t BaroAlt;
|
extern int32_t BaroAlt;
|
||||||
extern int32_t sonarAlt;
|
extern int32_t sonarAlt;
|
||||||
extern int32_t EstAlt;
|
extern int32_t EstAlt;
|
||||||
|
|
Loading…
Reference in New Issue