|
|
搞了那么久的定高，我只说说我用过MWC的飞控的定高程序，使用气压计定高的哈，对代码进行了注释。欢迎吐槽交流
#define BARO_TAB_SIZE 21
#define ACC_Z_DEADBAND (acc_1G>>5) // was 40 instead of 32 now
//死区计算
#define applyDeadband(value, deadband) \
if(abs(value) < deadband) { \
value = 0; \
} else if(value > 0){ \
value -= deadband; \
} else if(value < 0){ \
value += deadband; \
}
#if BARO
uint8_t getEstimatedAltitude(){
static uint32_t deadLine;
static int32_t baroGroundPressure;
static uint16_t previousT;
uint16_t currentT = micros();
uint16_t dTime;
dTime = currentT - previousT; // 时间
if (dTime < UPDATE_INTERVAL) return 0; //小于限定值，返回零
previousT = currentT;
if(calibratingB > 0) {
baroGroundPressure = baroPressureSum/(BARO_TAB_SIZE - 1);
calibratingB--;
} //获取气压值
// pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupi ... AP_Baro/AP_Baro.cpp
BaroAlt = log( baroGroundPressure * (BARO_TAB_SIZE - 1)/ (float)baroPressureSum ) * (baroTemperature+27315) * 29.271267f; // in cemtimeter 气压值转化到高度值 单位CM
EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs) 低通滤波
//定高程序
#if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
//P
int16_t error16 = constrain(AltHold - EstAlt, -300, 300); //计算差值
applyDeadband(error16, 10); //remove small P parametr to reduce noise near zero position 移除死区 降低噪声 在零点附近
BaroPID = constrain((conf.P8[PIDALT] * error16 >>7), -150, +150);
//计算P值
errorAltitudeI += conf.I8[PIDALT] * error16 >>6; //计算I值
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);//I值限幅
BaroPID += errorAltitudeI>>9; //I in range +/-60
//下面计算Z轴的加速度（公式 ） 计算每个方向的加速度在Z轴的投影
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
int16_t accZ = (accSmooth[ROLL] * EstG32.V.X + accSmooth[PITCH] * EstG32.V.Y + accSmooth[YAW] * EstG32.V.Z) * invG;
static int16_t accZoffset = 0; // = acc_1G*6; //58 bytes saved and convergence is fast enough to omit init
if (!f.ARMED) {
accZoffset -= accZoffset>>3;
accZoffset += accZ;
} //未解锁时 进行低通滤波
accZ -= accZoffset>>3;
applyDeadband(accZ, ACC_Z_DEADBAND);
static float vel = 0.0f;
static float accVelScale = 9.80665f / 10000.0f / acc_1G ;
// Integrator - velocity, cm/sec
vel += accZ * accVelScale * dTime;
//计算Z轴加速在该时间内在Z轴的对应的位移
static int32_t lastBaroAlt;
int16_t baroVel = (EstAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = EstAlt; //计算气压计上次的误差
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
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;
// 融合气压计和加速度计计算位移 这里采用加速度计的值和气压计的值融合后来做微分
//D
int16_t vel_tmp = vel;
applyDeadband(vel_tmp, 5);
vario = vel_tmp;
BaroPID -= constrain(conf.D8[PIDALT] * vel_tmp >>4, -150, 150);
//计算D值
#endif
return 1;
}
#endif //BARO // 看海冲浪 394171244@qq.com
|
|
发表于 2014-11-9 23:49:35
楼主
