接上篇。
IMU主要分为3大部分:
1. computeIMU ():提供给外的接口函数,也是传感器处理的总函数;
2. getEstimatedAttitude():获取估算的姿态,主要处理ACC、Gyro和Mag传感器数据;
3.getEstimatedAltitude():获取估算的高度,主要处理Baro传感器数据。
黑色为保留的代码,红色为可删除的多余代码。
/***************************************************************************************/
//1.
void computeIMU () {
uint8_t axis;
static int16_t gyroADCprevious[3] = {0,0,0};
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
static int16_t lastAccADC[3] = {0,0,0};
static uint32_t timeInterleave = 0;
static int16_t gyroYawSmooth = 0;
//we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
//gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
//gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
if (!ACC && nunchuk) {
annexCode();
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
WMP_getRawADC();
getEstimatedAttitude(); // computation time must last less than one interleaving delay
#if BARO
getEstimatedAltitude();
#endif
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
while(WMP_getRawADC() != 1) ; // For this interleaving reading, we must have a gyro update at this point (less delay)
for (axis = 0; axis < 3; axis++) {
// empirical, we take a weighted value of the current and the previous values
// /4 is to average 4 values, note: overflow is not possible for WMP gyro here
gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis]+2)/4;
gyroADCprevious[axis] = gyroADC[axis];
}
} else {
if (ACC) {
ACC_getADC(); //有加速度传感器
getEstimatedAttitude(); //估算姿态
if (BARO) getEstimatedAltitude(); //有气压传感器,估算高度
}
if (GYRO) Gyro_getADC(); else WMP_getRawADC();
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis]; //将从Sensors传来的数据保存
timeInterleave=micros();
annexCode(); //此函数代码后续再做探讨
while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads //给annexCode(); 运行时间
if (GYRO) Gyro_getADC(); else WMP_getRawADC();
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis]+gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis]+1)/3; //对最近三次的Gyro数据取平均值
gyroADCprevious[axis] = gyroADCinter[axis]/2;
if (!ACC) accADC[axis]=0;
}
}
#if defined(TRI)
gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW]+1)/3;
gyroYawSmooth = gyroData[YAW];
#endif
}
/***************************************************************************************/
//2
// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by http://starlino.com/imu_guide.html
//
// adapted by ziss_dm : http://wbb.multiwii.com/viewtopic.php?f=8&t=198
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// Last Modified: 19/04/2011
// Version: V1.1
// **************************************************
//****** advanced users settings *******************
/* Set the Low Pass Filter factor for ACC */
/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/
/* Comment this if you do not want filter at all.*/
/* Default WMC value: 8*/
#define ACC_LPF_FACTOR 8
/* Set the Low Pass Filter factor for Magnetometer */
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
/* Comment this if you do not want filter at all.*/
/* Default WMC value: n/a*/
//#define MG_LPF_FACTOR 4
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
/* Default WMC value: 300*/
#define GYR_CMPF_FACTOR 310.0f
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/
#define GYR_CMPFM_FACTOR 200.0f
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#if GYRO
#define GYRO_SCALE ((2000.0f * PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.155f)
// +-2000/sec deg scale
//#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)
// +- 200/sec deg scale
// 1.5 is emperical, not sure what it means
// should be in rad/sec
#else
#define GYRO_SCALE (1.0f/200e6f)
// empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility
// !!!!should be adjusted to the rad/sec
#endif
// Small angle approximation
#define ssin(val) (val)
#define scos(val) 1.0f
typedef struct {
float X;
float Y;
float Z;
} t_fp_vector_def;
typedef union {
float A[3];
t_fp_vector_def V;
} t_fp_vector;
int16_t _atan2(float y, float x){ //坐标(x,y)与x轴形成的角度
#define fp_is_neg(val) ((((byte*)&val)[3] & 0x80) != 0)
float z = y / x;
int16_t zi = abs(int16_t(z * 100));
int8_t y_neg = fp_is_neg(y);
if ( zi < 100 ){
if (zi > 10)
z = z / (1.0f + 0.28f * z * z);
if (fp_is_neg(x)) {
if (y_neg) z -= PI;
else z += PI;
}
} else {
z = (PI / 2.0f) - z / (z * z + 0.28f);
if (y_neg) z -= PI;
}
z *= (180.0f / PI * 10);
return z;
}
void getEstimatedAttitude(){ //估计姿态
uint8_t axis;
uint16_t accLim, accMag = 0;
static t_fp_vector EstG = {0,0,300};
static t_fp_vector EstM = {0,0,300};
float scale, deltaGyroAngle;
static int16_t mgSmooth[3]; //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer
static uint16_t previousT;
uint16_t currentT;
currentT = micros();
scale = (currentT - previousT) * GYRO_SCALE;
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
#if defined(ACC_LPF_FACTOR)
// accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR; // LPF for ACC values
accSmooth[axis] =(accSmooth[axis]*7+accADC[axis]+4)>>3;
#define ACC_VALUE accSmooth[axis]
#else
accSmooth[axis] = accADC[axis]; //对ACC数据进行滤波
#define ACC_VALUE accADC[axis]
#endif
accMag += (ACC_VALUE * 10 / acc_1G) * (ACC_VALUE * 10 / acc_1G); //788 // 加速度的数量级 ax*ax+ay*ay+az*az
#if MAG //对MAG进行滤波处理
#if defined(MG_LPF_FACTOR)
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth[axis]
#else
#define MAG_VALUE magADC[axis]
#endif
#endif
}
//根据Gyro数据算出angle[Roll] 和 angle[Pitch]
// Rotate Estimated vector(s), ROLL
deltaGyroAngle = gyroADC[ROLL] * scale;
EstG.V.Z = scos(deltaGyroAngle) * EstG.V.Z - ssin(deltaGyroAngle) * EstG.V.X;
EstG.V.X = ssin(deltaGyroAngle) * EstG.V.Z + scos(deltaGyroAngle) * EstG.V.X;
#if MAG
EstM.V.Z = scos(deltaGyroAngle) * EstM.V.Z - ssin(deltaGyroAngle) * EstM.V.X;
EstM.V.X = ssin(deltaGyroAngle) * EstM.V.Z + scos(deltaGyroAngle) * EstM.V.X;
#endif
// Rotate Estimated vector(s), PITCH
deltaGyroAngle = gyroADC[PITCH] * scale;
EstG.V.Y = scos(deltaGyroAngle) * EstG.V.Y + ssin(deltaGyroAngle) * EstG.V.Z;
EstG.V.Z = -ssin(deltaGyroAngle) * EstG.V.Y + scos(deltaGyroAngle) * EstG.V.Z;
#if MAG
EstM.V.Y = scos(deltaGyroAngle) * EstM.V.Y + ssin(deltaGyroAngle) * EstM.V.Z;
EstM.V.Z = -ssin(deltaGyroAngle) * EstM.V.Y + scos(deltaGyroAngle) * EstM.V.Z;
#endif
// Rotate Estimated vector(s), YAW
deltaGyroAngle = gyroADC[YAW] * scale;
EstG.V.X = scos(deltaGyroAngle) * EstG.V.X - ssin(deltaGyroAngle) * EstG.V.Y;
EstG.V.Y = ssin(deltaGyroAngle) * EstG.V.X + scos(deltaGyroAngle) * EstG.V.Y;
#if MAG
EstM.V.X = scos(deltaGyroAngle) * EstM.V.X - ssin(deltaGyroAngle) * EstM.V.Y;
EstM.V.Y = ssin(deltaGyroAngle) * EstM.V.X + scos(deltaGyroAngle) * EstM.V.Y;
#endif
//滤波,当加速度数量级不满足要求时候就舍弃数据
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
accLim = acc_1G*4/10;
if ( ( 36 < accMag && accMag < 196 ) || ( abs(accSmooth[ROLL])<accLim && abs(accSmooth[PITCH])<accLim ) ) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + ACC_VALUE) * INV_GYR_CMPF_FACTOR;
}
// Attitude of the estimated vector
angle[ROLL] = _atan2(EstG.V.X, EstG.V.Z) ;
angle[PITCH] = _atan2(EstG.V.Y, EstG.V.Z) ;
#if MAG //对磁场数据进行处理,定向功能
// Apply complimentary filter (Gyro drift correction)
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR - MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
// Attitude of the cross product vector GxM
heading = _atan2(EstG.V.Z * EstM.V.X - EstG.V.X * EstM.V.Z, EstG.V.Y * EstM.V.Z - EstG.V.Z * EstM.V.Y) / 10;
#endif
}
/***************************************************************************************/
//3.
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;}
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define Kp1 0.55f // PI observer velocity gain
#define Kp2 1.0f // PI observer position gain
#define Ki 0.001f // PI observer integral gain (bias cancellation)
#define dt (UPDATE_INTERVAL / 1000000.0f)
//对气压计数据进行处理,高度估算。
void getEstimatedAltitude(){
static uint8_t inited = 0;
static float AltErrorI = 0.0f;
static float AccScale = 0.0f;
static uint32_t DeadLine = INIT_DELAY;
float AltError;
float InstAcc = 0.0f;
float Delta;
if (currentTime < DeadLine) return;
DeadLine = currentTime + UPDATE_INTERVAL;
// Soft start
if (!inited) {
inited = 1;
EstAlt = BaroAlt;
EstVelocity = 0.0f;
AltErrorI = 0.0f;
AccScale = (9.80665f / acc_1G);
}
// Estimation Error
AltError = BaroAlt - EstAlt;
AltErrorI += AltError;
// Gravity vector correction and projection to the local Z
InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + (Ki) * AltErrorI;
// Integrators
Delta = InstAcc * dt + (Kp1 * dt) * AltError;
EstAlt += ((EstVelocity + Delta * 0.5f) * dt + (Kp2 * dt) * AltError);
EstVelocity += Delta;
}
附图:
其实IMU程序写的还是有些不足之处的。
1. EstG.A[i]和EstM.A[i]用于ACC和MAG的数据融合和滤波,但其实根本没有起作用;
2.computeIMU其实应该在确定定义了GYRO传感器之后再进行姿态估计。
3. 。。。
总之,我分析的是MultiWii_1_8版本,高版本的代码更加完善与健壮,但大体思路应该相差不大。