论坛大神
  
- 积分
- 1848
- 金钱
- 1848
- 注册时间
- 2013-4-15
- 在线时间
- 163 小时
|
楼主 |
发表于 2014-4-17 14:10:51
|
显示全部楼层
回复【4楼】Badu_Space:
---------------------------------
#include "usart.h"
#include "sys.h"
#include "delay.h"
#include "hmc5883l.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "inv_mpu.h"
#include "Time.h"
#include "myiic.h"
#include "math.h"
#include "usb_lib.h"
#include "hw_config.h"
#include "usb_pwr.h"
#define Cursor_Step 1
//定义不同测量范围的刻度因子
#define Accel_4_Scale_Factor 8192.0f
#define FILTER_NUM 20
#define Accel_Zout_Offset 400
#define Accel_FILTER_98HZ 98
#define MPU6050_Addr 0x68
#define  _M_1 0x6B
#define Gyro_Xout_H 0x43
#define Accel_Xout_H 0x3B
#define Kp 2.0f //proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f //integral gain governs rate of convergence of gyroscope biases
u8 data_write[14];
float Gyro_Xout_Offset, Gyro_Yout_Offset, Gyro_Zout_Offset;
float q0, q1, q2, q3;
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
float init_ax, init_ay, init_az, init_gx, init_gy, init_gz, init_mx, init_my, init_mz;
u8 get_gyro_bias(void); //校准陀螺仪零偏函数
void init_quaternion(void); //得到初始化四元数函数
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); //AHRSUpdate算法
float invSqrt(float x); //快速计算 1/Sqrt(x)
int main(void)
{
int result;
SystemInit();
delay_init(); //SYSTICK的时钟固定为HCLK时钟的1/8,SYSCLK:系统时钟为72MHz.
uart_init(9600); //串口1初始化,波特率为9600,工作在全双工收发模式
NVIC_Configuration();
//USB配置
USB_Interrupts_Config();
Set_USBClock();
USB_Init();
SYSTICK_INIT();
IIC_Init();
delay_ms(10);
HMC5883L_Init();
PrintChar("hmc5883l initialization complete......\n "); //hmc5883 initialization complete
HMC5883L_Calibrate();
PrintChar("hmc5883l_calibrate initialization complete......\n "); //hmc5883 initialization complete
result = mpu_init();
if(!result)
{
PrintChar("mpu initialization complete......\n "); //mpu initialization complete
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //选择所需传感器
PrintChar("mpu_set_sensor complete ......\n");
else
PrintChar("mpu_set_sensor come across error ......\n");
data_write[0]=0x01; //GX_PLL:0x01
if(!IIC_Write(MPU6050_Addr, _M_1, 1, data_write)) //设置MPU6050的时钟源为GX的PLL
PrintChar("mpu6050_set_ClockSource complete ......\n");
else
PrintChar("mpu6050_set_ClockSource error ......\n");
if(!mpu_set_gyro_fsr(500)) //设置陀螺仪测量范围：+-500度/s
PrintChar("mpu_set_gyro_fsr complete ......\n");
else
PrintChar("mpu_set_gyro_fsr error ......\n");
if(!mpu_set_accel_fsr(4)) //设置加速度计测量范围：+-4G
PrintChar("mpu_set_accel_fsr complete ......\n");
else
PrintChar("mpu_set_accel_fsr error ......\n");
if(!mpu_set_lpf(Accel_FILTER_98HZ)) //设置加速度计的低通滤波器DLPF选98hz，
//后面还需要加权平均滤波
PrintChar("mpu_set_lpf complete ......\n");
else
PrintChar("mpu_set_lpf error ......\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //设置采样率1kHz
PrintChar("mpu_set_sample_rate complete ......\n");
else
PrintChar("mpu_set_sample_rate error ......\n");
if(!get_gyro_bias()) //通过500次静置加权平均求出陀螺仪零偏OFFSET
PrintChar("get_gyro_bias complete ......\n");
else
PrintChar("get_gyro_bias error ......\n");
}
else
PrintChar("mpu initialization error......\n");
PrintChar("Start Calculating .....\n"); //开始计算
delay_ms(2000); //2s后系统稳定
init_quaternion(); //得到初始化四元数
while(1)
{
uint8_t i;
static uint8_t j;
signed short int gyro[3], accel[3];
static tg_HMC5883L_TYPE hmc5883l;
static u8 filter_cnt=0;
float temp[3]={0};
float ACC_X_BUF[FILTER_NUM], ACC_Y_BUF[FILTER_NUM], ACC_Z_BUF[FILTER_NUM];
if(!IIC_Read(MPU6050_Addr, Accel_Xout_H, 14, data_write))
{
accel[0]=(((signed short int)data_write[0])<<8) | data_write[1];
accel[1]=(((signed short int)data_write[2])<<8) | data_write[3];
accel[2]=((((signed short int)data_write[4])<<8) | data_write[5]) + Accel_Zout_Offset;
gyro[0] =((((signed short int)data_write[8])<<8) | data_write[9]);
gyro[1] =((((signed short int)data_write[10])<<8) | data_write[11]);
gyro[2] =((((signed short int)data_write[12])<<8) | data_write[13]);
init_ax=(float)accel[0];
init_ay=(float)accel[1];
init_az=(float)accel[2];
init_gx=((float)gyro[0] - Gyro_Xout_Offset) * 0.000266; //单位转化成：弧度/s，0.000266=1/(Gyro_500_Scale_Factor * 57.295780)
init_gy=((float)gyro[1] - Gyro_Yout_Offset) * 0.000266;
init_gz=((float)gyro[2] - Gyro_Zout_Offset) * 0.000266;
//accel加权平均滤波，部分消除震动影响
ACC_X_BUF[filter_cnt] = init_ax;//更新滑动窗口数组
ACC_Y_BUF[filter_cnt] = init_ay;
ACC_Z_BUF[filter_cnt] = init_az;
for(i=0;i<FILTER_NUM;i++)
{
temp[0] += ACC_X_BUF;
temp[1] += ACC_Y_BUF;
temp[2] += ACC_Z_BUF;
}
init_ax = temp[0] * 0.05; // 相当于除以FILTER_NUM，提高运算速度
init_ay = temp[1] * 0.05;
init_az = temp[2] * 0.05;
filter_cnt++;
if(filter_cnt==FILTER_NUM)
filter_cnt=0;
j++;
if(j==50)
{
j=0;
HMC5883L_Init();
delay_ms(10);
HMC5883L_MultRead(&hmc5883l); //读磁阻仪数据(速度:慢, ms级延时过程)
}
//进行x y轴的校准，未对z轴进行校准，参考ST的校准方法
init_mx =hmc5883l.hx;
init_my =hmc5883l.hy;
init_mz =hmc5883l.hz;
AHRSupdate(init_gx, init_gy, init_gz, init_ax, init_ay, init_az, init_mx, init_my, init_mz);
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float norm, halfT;
float hx, hy, hz, bz, bx;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
// float itch, Roll, Yaw;
static s8 sb_x,sb_y,sb_z;
/*方便之后的程序使用，减少计算时间*/
//auxiliary variables to reduce number of repeated operations，
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
/*归一化测量值，加速度计和磁力计的单位是什么都无所谓，因为它们在此被作了归一化处理*/
//normalise the measurements
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
norm = invSqrt(mx*mx + my*my + mz*mz);
mx = mx * norm;
my = my * norm;
mz = mz * norm;
/*从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz（测量值），下面这个是从飞行器坐标系到世界坐标系的转换公式*/
//compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
/*计算地理坐标系下的磁场矢量bxyz（参考值）。
因为地理地磁水平夹角，我们已知是0度（抛去磁偏角的因素，固定向北），我定义by指向正北，所以by=某值，bx=0
但地理参考地磁矢量在垂直面上也有分量bz，地球上每个地方都是不一样的。
我们无法得知，也就无法用来融合（有更适合做垂直方向修正融合的加速度计），所以直接从测量值hz上复制过来，bz=hz。
磁场水平分量，参考值和测量值的大小应该是一致的(bx*bx) + (by*by)) = ((hx*hx) + (hy*hy))。
因为bx=0，所以就简化成(by*by) = ((hx*hx) + (hy*hy))。可算出by。这里修改by和bx指向可以定义哪个轴指向正北*/
bx = sqrtf((hx*hx) + (hy*hy));
//by = sqrtf((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)，下面这个是从世界坐标系到飞行器坐标系的转换公式(转置矩阵)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
/*我们把地理坐标系上的磁场矢量bxyz，转到机体上来wxyz。
因为bx=0，所以所有涉及到bx的部分都被省略了。同理by=0，所以所有涉及到by的部分也可以被省略，这根据自己定义那个轴指北有关。
类似上面重力vxyz的推算，因为重力g的az=1，ax=ay=0，所以上面涉及到gxgy的部分也被省略了
你可以看看两个公式：wxyz的公式，把by换成ay（0），把bz换成az（1），就变成了vxyz的公式了（其中q0q0+q1q1+q2q2+q3q3=1）。*/
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// wx = 2*by*(q1q2 + q0q3) + 2*bz*(q1q3 - q0q2);
// wy = 2*by*(0.5 - q1q1 - q3q3) + 2*bz*(q0q1 + q2q3);
// wz = 2*by*(q2q3 - q0q1) + 2*bz*(0.5 - q1q1 - q2q2);
//现在把加速度的测量矢量和参考矢量做叉积，把磁场的测量矢量和参考矢量也做叉积。都拿来来修正陀螺。
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// // integral error scaled integral gain
// exInt = exInt + ex*Ki;
// eyInt = eyInt + ey*Ki;
// ezInt = ezInt + ez*Ki;
// // adjusted gyroscope measurements
// gx = gx + Kp*ex + exInt;
// gy = gy + Kp*ey + eyInt;
// gz = gz + Kp*ez + ezInt;
halfT=GET_NOWTIME(); //得到每次姿态更新的周期的一半
if(ex != 0.0f && ey != 0.0f && ez != 0.0f) //很关键的一句话，原算法没有
{
// integral error scaled integral gain
exInt = exInt + ex*Ki * halfT; //乘以采样周期的一半
eyInt = eyInt + ey*Ki * halfT;
ezInt = ezInt + ez*Ki * halfT;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
}
if(gx > (+1.5))
sb_x += Cursor_Step;
if(gx < (-1.5))
sb_x -= Cursor_Step;
if(gy > (+1.0))
sb_y += Cursor_Step;
if(gy < (-2.5))
sb_y -= Cursor_Step;
if(gz > (+1.5))
sb_z += Cursor_Step;
if(gz < (-1.5))
sb_z -= Cursor_Step;
Joystick_Send(0,sb_x,sb_y,0); //发送数据到电脑
} |
|
/2 
|手机版|OpenEdv-开源电子网
( 粤ICP备12000418号-1 )
发表于 2014-4-17 12:44:12
