添加mpu9250驱动测试代码
parent
29a35471f7
commit
3e6fa2cdde
|
|
@ -13,3 +13,18 @@
|
|||
*.dep
|
||||
*.o
|
||||
*.crf
|
||||
*.siwork
|
||||
*.xml
|
||||
*.siproj
|
||||
*.sip_xsd
|
||||
*.sip_xsb
|
||||
*.sip_xr
|
||||
*.sip_xm
|
||||
*.sip_xf
|
||||
*.sip_xc
|
||||
*.sip_xad
|
||||
*.sip_xab
|
||||
*.sip_sym
|
||||
*.SearchResults
|
||||
*.sisc
|
||||
*.iex
|
||||
|
|
|
|||
File diff suppressed because one or more lines are too long
|
|
@ -48,7 +48,7 @@ void OLED_SingleWrite(uint8_t index, uint8_t data)
|
|||
|
||||
uint8_t OLED_SingleRead(uint8_t index)
|
||||
{
|
||||
uint8_t tmp;
|
||||
uint8_t tmp;
|
||||
I2C_START(LCD_I2C_PORT); //Start
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
//LCD_I2C_PORT->INTSTS |= I2C_INTSTS_INTSTS_Msk; //clear flag
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
|
@ -2,11 +2,14 @@
|
|||
".\objects\main.o"
|
||||
".\objects\hal.o"
|
||||
".\objects\mpu.o"
|
||||
".\objects\imu.o"
|
||||
".\objects\mymath.o"
|
||||
".\objects\clk.o"
|
||||
".\objects\i2c.o"
|
||||
".\objects\timer.o"
|
||||
".\objects\retarget.o"
|
||||
".\objects\startup_m451series.o"
|
||||
".\objects\system_m451series.o"
|
||||
".\objects\clk.o"
|
||||
".\objects\i2c.o"
|
||||
--ro-base 0x00000000 --entry 0x00000000 --rw-base 0x20000000 --entry Reset_Handler --first __Vectors --strict --summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols
|
||||
--info sizes --info totals --info unused --info veneers
|
||||
--list ".\Listings\mpu9250.map" -o .\Objects\mpu9250.axf
|
||||
|
|
@ -18,5 +18,6 @@
|
|||
|
||||
#define RTE_Drivers_CLK /* Driver CLK */
|
||||
#define RTE_Drivers_I2C /* Driver I2C */
|
||||
#define RTE_Drivers_Timer /* Driver Timer */
|
||||
|
||||
#endif /* RTE_COMPONENTS_H */
|
||||
|
|
|
|||
|
|
@ -27,8 +27,37 @@ typedef struct{
|
|||
int16_t gyroX;
|
||||
int16_t gyroY;
|
||||
int16_t gyroZ;
|
||||
int16_t magX;
|
||||
int16_t magY;
|
||||
int16_t magZ;
|
||||
}_st_Mpu;
|
||||
|
||||
typedef struct {
|
||||
float accX;
|
||||
float accY;
|
||||
float accZ;
|
||||
float gyroX;
|
||||
float gyroY;
|
||||
float gyroZ;
|
||||
float magX;
|
||||
float magY;
|
||||
float magZ;
|
||||
}_st_Mpu_Angle;
|
||||
|
||||
struct _1_ekf_filter
|
||||
{
|
||||
float LastP;
|
||||
float Now_P;
|
||||
float out;
|
||||
float Kg;
|
||||
float Q;
|
||||
float R;
|
||||
};
|
||||
|
||||
//void ekf_1(struct EKF *ekf,void *input); //一维å<C2B4>¡å°”曼
|
||||
extern void kalman_1(struct _1_ekf_filter *ekf,float input); //一维å<C2B4>¡å°”曼
|
||||
|
||||
|
||||
|
||||
typedef struct{
|
||||
int16_t magX;
|
||||
|
|
@ -93,7 +122,9 @@ typedef volatile struct
|
|||
|
||||
extern _st_Remote Remote;
|
||||
extern _st_Mpu MPU6050;
|
||||
extern _st_Mag AK8975; //保留，需外接磁力计
|
||||
extern _st_Mpu MPU6050UnFiltered;
|
||||
extern _st_Mpu_Angle MPU6050Angle;
|
||||
extern _st_Mag AK8975; //ä¿<C3A4>留，需外接ç£<C3A7>力è®?
|
||||
extern _st_AngE Angle;
|
||||
|
||||
|
||||
|
|
@ -111,5 +142,10 @@ extern PidObject pidYaw;
|
|||
extern PidObject pidHeightRate;
|
||||
extern PidObject pidHeightHigh;
|
||||
|
||||
extern int i2c0Lock;
|
||||
|
||||
extern void I2C0_LCK ();
|
||||
extern void I2C0_UNLOCK();
|
||||
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -2,228 +2,134 @@
|
|||
#include "M451Series.h"
|
||||
#include <stdio.h>
|
||||
#include "hal.h"
|
||||
#define LCD_I2C_PORT I2C0
|
||||
|
||||
int MPUWriteReg( char RegAddr, char pucDATD_AA)
|
||||
int MPUWriteReg(char ad,char RegAddr, char pucDATD_AA)
|
||||
{
|
||||
int i=0;
|
||||
|
||||
while(i<32) i++;
|
||||
I2C_START(LCD_I2C_PORT); //Start
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
//LCD_I2C_PORT->INTSTS |= I2C_INTSTS_INTSTS_Msk; //clear flag
|
||||
|
||||
I2C_START(I2C0);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x08)
|
||||
{
|
||||
printf("I2CD_STArt write fail,I2D_STATUS %02X\r\n",I2C_GET_STATUS(I2C0));
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_SET_DATA(LCD_I2C_PORT, ad); //send slave address
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_SI);
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
//LCD_I2C_PORT->INTSTS |= I2C_INTSTS_INTSTS_Msk; //clear flag
|
||||
|
||||
I2C_SET_DATA(I2C0,0xd0);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0)!= 0x18)
|
||||
{
|
||||
printf("I2C write ADW fail\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
//дÈë¶ÁµØÖ·
|
||||
I2C_SET_DATA(I2C0,RegAddr);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x28)
|
||||
{
|
||||
printf("I2C write reg addr fail\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_SET_DATA(LCD_I2C_PORT, RegAddr); //send index
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_SI);
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
|
||||
//LCD_I2C_PORT->INTSTS |= I2C_INTSTS_INTSTS_Msk; //clear flag
|
||||
|
||||
I2C_SET_DATA(I2C0,pucDATD_AA);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x28)
|
||||
{
|
||||
printf("I2C write control fail\r\n");
|
||||
while (1);
|
||||
}
|
||||
|
||||
//Í£Ö¹
|
||||
I2C_Trigger(I2C0,0,1,1,0);
|
||||
|
||||
|
||||
//printf("I2C write ok\r\n");
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
int MPUWriteAddr()
|
||||
{
|
||||
if (I2C_GET_STATUS(I2C0) != 0x08)
|
||||
{
|
||||
printf("I2CD_STArt write add fail,I2D_STATUS %02X\r\n",I2C_GET_STATUS(I2C0));
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
//½øÈë¶Áд¿ØÖƲÙ×÷
|
||||
I2C_SET_DATA(I2C0,0xd0);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0)!= 0x18)
|
||||
{
|
||||
printf("I2C write ADW fail\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
return 1;
|
||||
I2C_SET_DATA(LCD_I2C_PORT, pucDATD_AA); //send Data
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_SI);
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
//LCD_I2C_PORT->INTSTS |= I2C_INTSTS_INTSTS_Msk; //clear flag
|
||||
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_SI|I2C_CTL_STO);//Stop
|
||||
}
|
||||
|
||||
|
||||
int MPUWriteACK(char cDat)
|
||||
{
|
||||
if((I2C_GET_STATUS(I2C0) != 0x18)&&(I2C_GET_STATUS(I2C0) != 0x28))
|
||||
{
|
||||
printf("I2C MPUWriteAddrAck STATUS error \r\n");
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
//дÈë¶ÁµØÖ·
|
||||
I2C_SET_DATA(I2C0,cDat);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0)!= 0x28)
|
||||
{
|
||||
printf("MPUWriteAddrAck fail ACK no recv\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
char MPUReadReg( int unAddr/*, int unLength*/)
|
||||
{
|
||||
char ret;
|
||||
int i=0;
|
||||
char MPUReadReg(char ad,int unAddr/*, int unLength*/){
|
||||
|
||||
uint8_t tmp;
|
||||
I2C_START(LCD_I2C_PORT); //Start
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
//LCD_I2C_PORT->INTSTS |= I2C_INTSTS_INTSTS_Msk; //clear flag
|
||||
|
||||
I2C_SET_DATA(LCD_I2C_PORT,ad ); //send slave address+W
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_SI);
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
|
||||
I2C_SET_DATA(LCD_I2C_PORT, unAddr); //send index
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_SI);
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
|
||||
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_STA | I2C_CTL_SI); //Start
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
|
||||
|
||||
while(i<32) i++;
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
|
||||
I2C_START(I2C0); //Æô¶¯
|
||||
//Æô¶¯
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if(I2C_GET_STATUS(I2C0) != 0x08)
|
||||
{
|
||||
printf("I2CD_STArt read reg fail,I2D_STATUS %02X\r\n",I2C_GET_STATUS(I2C0));
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
|
||||
//½øÈë¶Áд¿ØÖƲÙ×÷
|
||||
I2C_SET_DATA(I2C0,0xd0);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x018)
|
||||
{
|
||||
printf("status fault shoube be 0x018 ,I2D_STATUS %02X\r\n",I2C_GET_STATUS(I2C0));
|
||||
return FALSE;
|
||||
}
|
||||
//дÈë¶ÁµØÖ·
|
||||
I2C_SET_DATA(I2C0,unAddr);
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0)!= 0x28)
|
||||
{
|
||||
printf("I2C write reg addr fail\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
// ÖØÐÂÆô¶¯
|
||||
|
||||
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_Trigger(I2C0,1,0,0,0);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x10)
|
||||
{
|
||||
printf("I2C repeated D_STArt fail\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
|
||||
//½øÈë¶Á²Ù×÷
|
||||
I2C_SET_DATA(I2C0,0xd0 | 1);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x40)
|
||||
{
|
||||
printf("I2C write control fail\r\n");
|
||||
while (1);
|
||||
}
|
||||
//¶ÁÈ¡Êý¾Ý
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x58)
|
||||
{
|
||||
printf("I2C read fail\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
ret = I2C_GET_DATA(I2C0);
|
||||
I2C_Trigger(I2C0,0,1,1,0);
|
||||
I2C_SET_DATA(LCD_I2C_PORT, (ad+1)); //send slave address+R
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_SI);
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_SI);
|
||||
I2C_WAIT_READY(LCD_I2C_PORT);
|
||||
|
||||
// I2C_WAIT_READY(I2C0);
|
||||
return ret;
|
||||
tmp = I2C_GET_DATA(LCD_I2C_PORT); //read data
|
||||
|
||||
I2C_SET_CONTROL_REG(LCD_I2C_PORT, I2C_CTL_SI|I2C_CTL_STO);//Stop
|
||||
return tmp;
|
||||
}
|
||||
|
||||
int MPUReadBuf( int unAddr, char *pucDATD_AA, int unLength)
|
||||
int MPUReadBuf(char slaveAddress,int unAddr, char *pucDATD_AA, int unLength)
|
||||
{
|
||||
char ret;
|
||||
int i=0;
|
||||
while(i<32) i++;
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_START(I2C0);
|
||||
I2C_START(I2C0); //启动
|
||||
//启动
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if(I2C_GET_STATUS(I2C0) != 0x08)
|
||||
{
|
||||
printf("I2CD_STArt fail,I2D_STATUS %02X\r\n",I2C_GET_STATUS(I2C0));
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_SET_CONTROL_REG(I2C0, I2C_CTL_SI);
|
||||
|
||||
I2C_SET_DATA(I2C0,0xd0);
|
||||
//进入读写控制操作
|
||||
I2C_SET_DATA(I2C0,slaveAddress);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x18)
|
||||
{
|
||||
printf("I2CD_STArt fail,I2D_STATUS %02X\r\n",I2C_GET_STATUS(I2C0));
|
||||
return FALSE;
|
||||
}
|
||||
//写入读地址
|
||||
I2C_SET_DATA(I2C0,unAddr);
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_SET_CONTROL_REG(I2C0, I2C_CTL_SI);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0)!= 0x28)
|
||||
{
|
||||
printf("I2C write reg addr fail\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_Trigger(I2C0,1,0,0,0);
|
||||
}
|
||||
// 重新启动
|
||||
|
||||
I2C_SET_CONTROL_REG(I2C0, I2C_CTL_SI);
|
||||
I2C_SET_CONTROL_REG(I2C0, I2C_CTL_STA);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x10)
|
||||
{
|
||||
printf("I2C repeated D_STArt fail\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_SET_DATA(I2C0,0xd0 | 1);
|
||||
I2C_SET_CONTROL_REG(I2C0, I2C_CTL_SI);
|
||||
|
||||
//进入读操作
|
||||
I2C_SET_DATA(I2C0,slaveAddress | 1);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if (I2C_GET_STATUS(I2C0) != 0x40)
|
||||
{
|
||||
printf("I2C write control fail\r\n");
|
||||
while (1);
|
||||
return -1;
|
||||
}
|
||||
for(i=0;i<unLength;i++)
|
||||
//读取数据
|
||||
for(int i=0;i<unLength;i++)
|
||||
{
|
||||
if(i==unLength-1)
|
||||
I2C_Trigger(I2C0,0,0,1,0);
|
||||
I2C_SET_CONTROL_REG(I2C0, I2C_CTL_SI);
|
||||
else
|
||||
I2C_Trigger(I2C0,0,0,1,1);
|
||||
I2C_SET_CONTROL_REG(I2C0, I2C_CTL_SI | I2C_CTL_SI_AA);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
if ((I2C_GET_STATUS(I2C0) != 0x58)&&(I2C_GET_STATUS(I2C0) != 0x50))
|
||||
if ((I2C_GET_STATUS(I2C0) != 0x50))
|
||||
{
|
||||
printf("I2C read fail\r\n");
|
||||
return FALSE;
|
||||
}
|
||||
pucDATD_AA[i] = I2C_GET_DATA(I2C0);
|
||||
}
|
||||
I2C_Trigger(I2C0,0,1,1,0);
|
||||
I2C_SET_CONTROL_REG(I2C0, I2C_CTL_STO);
|
||||
I2C_WAIT_READY(I2C0);
|
||||
|
||||
|
||||
// I2C_WAIT_READY(I2C0);
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -1,10 +1,10 @@
|
|||
#ifndef __HAL__
|
||||
#define __HAL__
|
||||
|
||||
int MPUWriteReg( char RegAddr, char pucDATD_AA);
|
||||
int MPUWriteReg(char ad,char RegAddr, char pucDATD_AA);
|
||||
int MPUWriteAddr();
|
||||
char MPUReadReg( int unAddr/*, int unLength*/);
|
||||
int MPUReadBuf( int unAddr, char *pucDATD_AA, int unLength);
|
||||
char MPUReadReg( int unAddr/*, int unLength*/);
|
||||
char MPUReadReg(char ad,int unAddr/*, int unLength*/);
|
||||
int MPUReadBuf(char slaveAddress,int unAddr, char *pucDATD_AA, int unLength);
|
||||
void MpuGetData(void); //读取陀螺仪数据加滤波
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,263 @@
|
|||
/******************************************************************************************************
|
||||
* Update the PID parameters.
|
||||
*
|
||||
* @param[in] pid A pointer to the pid object.
|
||||
* @param[in] measured The measured value
|
||||
* @param[in] updateError Set to TRUE if error should be calculated.
|
||||
* Set to False if pidSetError() has been used.
|
||||
* @return PID algorithm output
|
||||
*******************************************************************************************************/
|
||||
#include "imu.h"
|
||||
#include "myMath.h"
|
||||
#include <math.h>
|
||||
|
||||
|
||||
static float NormAcc;
|
||||
|
||||
|
||||
//float ex_int = 0, ey_int = 0, ez_int = 0; //X、Y、Z轴的比例误差
|
||||
//float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //定义四元素
|
||||
//float his_q0 = 1, his_q1 = 0, his_q2 = 0, his_q3 = 0;
|
||||
//float q0_yaw = 1, q1_yaw = 0, q2_yaw = 0, q3_yaw = 0; //弥补Mahony算法在无地磁情况解算Yaw轴满足不了大扰动要求的现象
|
||||
//float his_q0_yaw = 1, his_q1_yaw = 0, his_q2_yaw = 0, his_q3_yaw = 0;
|
||||
|
||||
//void GetAngle(const stMpu *pMpu,void *pAngE, float dt)
|
||||
//{
|
||||
// static const float KpDef = 0.85f ;
|
||||
// static const float KiDef = 0.0035f;
|
||||
// const float Gyro_Gr = 0.0005326f; //面计算度每秒,转换弧度每秒则 0.03051756 * 0.0174533f = 0.0005326
|
||||
// float HalfTime = 0.5 * dt;
|
||||
// float gx = pMpu->gyroX * Gyro_Gr;
|
||||
// float gy = pMpu->gyroY * Gyro_Gr;
|
||||
// float gz = pMpu->gyroZ * Gyro_Gr;
|
||||
// float ax = pMpu->accX;
|
||||
// float ay = pMpu->accY;
|
||||
// float az = pMpu->accZ;
|
||||
//
|
||||
// float vx, vy, vz;
|
||||
// float ex, ey, ez;
|
||||
//
|
||||
// static float his_q0q0;
|
||||
// static float his_q0q1;
|
||||
// static float his_q0q2;
|
||||
// static float his_q1q1;
|
||||
// static float his_q1q3;
|
||||
// static float his_q2q2;
|
||||
// static float his_q2q3;
|
||||
// static float his_q3q3;
|
||||
//
|
||||
// float q0q0;
|
||||
// float q0q1;
|
||||
// float q0q2;
|
||||
// float q1q1;
|
||||
// float q1q3;
|
||||
// float q2q2;
|
||||
// float q2q3;
|
||||
// float q3q3;
|
||||
//
|
||||
// float q0_yawq0_yaw;
|
||||
// float q1_yawq1_yaw;
|
||||
// float q2_yawq2_yaw;
|
||||
// float q3_yawq3_yaw;
|
||||
// float q1_yawq2_yaw;
|
||||
// float q0_yawq3_yaw;
|
||||
//
|
||||
////**************************Yaw轴计算******************************
|
||||
//
|
||||
// //Yaw轴四元素的微分方程
|
||||
// q0_yaw = his_q0_yaw + (-his_q1_yaw * gx - his_q2_yaw * gy - his_q3_yaw * gz) * HalfTime;
|
||||
// q1_yaw = his_q1_yaw + ( his_q0_yaw * gx + his_q2_yaw * gz - his_q3_yaw * gy) * HalfTime;
|
||||
// q2_yaw = his_q2_yaw + ( his_q0_yaw * gy - his_q1_yaw * gz + his_q3_yaw * gx) * HalfTime;
|
||||
// q3_yaw = his_q3_yaw + ( his_q0_yaw * gz + his_q1_yaw * gy - his_q2_yaw * gx) * HalfTime;
|
||||
//
|
||||
// q0_yawq0_yaw = q0_yaw * q0_yaw;
|
||||
// q1_yawq1_yaw = q1_yaw * q1_yaw;
|
||||
// q2_yawq2_yaw = q2_yaw * q2_yaw;
|
||||
// q3_yawq3_yaw = q3_yaw * q3_yaw;
|
||||
// q1_yawq2_yaw = q1_yaw * q2_yaw;
|
||||
// q0_yawq3_yaw = q0_yaw * q3_yaw;
|
||||
//
|
||||
// //规范化Yaw轴四元数
|
||||
// norm = Q_rsqrt(q0_yawq0_yaw + q1_yawq1_yaw + q2_yawq2_yaw + q3_yawq3_yaw);
|
||||
// q0_yaw = q0_yaw * norm;
|
||||
// q1_yaw = q1_yaw * norm;
|
||||
// q2_yaw = q2_yaw * norm;
|
||||
// q3_yaw = q3_yaw * norm;
|
||||
//
|
||||
// //if(ax * ay * az == 0)
|
||||
// //return ;
|
||||
//
|
||||
// //归一化加速度计值
|
||||
// norm = Q_rsqrt(ax * ax + ay * ay + az * az);
|
||||
// ax = ax * norm;
|
||||
// ay = ay * norm;
|
||||
// az = az * norm;
|
||||
//
|
||||
// //由姿态阵估计重力方向和流量/变迁
|
||||
// vx = 2 * (his_q1q3 - his_q0q2);
|
||||
// vy = 2 * (his_q0q1 + his_q2q3);
|
||||
// vz = his_q0q0 - his_q1q1 - his_q2q2 + his_q3q3;
|
||||
//
|
||||
// //向量外积再相减得到差分就是误差 两个单位向量的差积即为误差向量
|
||||
// ex = (ay * vz - az * vy) ;
|
||||
// ey = (az * vx - ax * vz) ;
|
||||
// ez = (ax * vy - ay * vx) ;
|
||||
|
||||
// //对误差进行PI计算
|
||||
// ex_int = ex_int + ex * KiDef;
|
||||
// ey_int = ey_int + ey * KiDef;
|
||||
// ez_int = ez_int + ez * KiDef;
|
||||
|
||||
// //校正陀螺仪
|
||||
// gx = gx + KpDef * ex + ex_int;
|
||||
// gy = gy + KpDef * ey + ey_int;
|
||||
// gz = gz + KpDef * ez + ez_int;
|
||||
//
|
||||
// //四元素的微分方程
|
||||
// q0 = his_q0 + (-his_q1 * gx - his_q2 * gy - his_q3 * gz) * HalfTime;
|
||||
// q1 = his_q1 + ( his_q0 * gx + his_q2 * gz - his_q3 * gy) * HalfTime;
|
||||
// q2 = his_q2 + ( his_q0 * gy - his_q1 * gz + his_q3 * gx) * HalfTime;
|
||||
// q3 = his_q3 + ( his_q0 * gz + his_q1 * gy - his_q2 * gx) * HalfTime;
|
||||
|
||||
// q0q0 = q0 * q0;
|
||||
// q0q1 = q0 * q1;
|
||||
// q0q2 = q0 * q2;
|
||||
// q1q1 = q1 * q1;
|
||||
// q1q3 = q1 * q3;
|
||||
// q2q2 = q2 * q2;
|
||||
// q2q3 = q2 * q3;
|
||||
// q3q3 = q3 * q3;
|
||||
|
||||
// //规范化Pitch、Roll轴四元数
|
||||
// norm = Q_rsqrt(q0q0 + q1q1 + q2q2 + q3q3);
|
||||
// q0 = q0 * norm;
|
||||
// q1 = q1 * norm;
|
||||
// q2 = q2 * norm;
|
||||
// q3 = q3 * norm;
|
||||
//
|
||||
// //求解欧拉角
|
||||
// *((float *)pAngE+2) = atan2f(2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1) * RtA; //ROLL
|
||||
// *((float *)pAngE+1) = asin(2 * q0q2 -2 * q1q3) * 57.3; //PITCH
|
||||
// *(float *)pAngE = atan2f(2 * q1_yawq2_yaw + 2 * q0_yawq3_yaw, -2 * q2_yawq2_yaw - 2 * q3_yawq3_yaw + 1) * RtA; //YAW
|
||||
//
|
||||
//
|
||||
// //存储更替相应的四元数
|
||||
// his_q0_yaw = q0_yaw;
|
||||
// his_q1_yaw = q1_yaw;
|
||||
// his_q2_yaw = q2_yaw;
|
||||
// his_q3_yaw = q3_yaw;
|
||||
|
||||
// his_q0 = q0;
|
||||
// his_q1 = q1;
|
||||
// his_q2 = q2;
|
||||
// his_q3 = q3;
|
||||
//
|
||||
// his_q0q0 = q0q0;
|
||||
// his_q0q1 = q0q1;
|
||||
// his_q0q2 = q0q2;
|
||||
// his_q1q1 = q1q1;
|
||||
// his_q1q3 = q1q3;
|
||||
// his_q2q2 = q2q2;
|
||||
// his_q2q3 = q2q3;
|
||||
// his_q3q3 = q3q3;
|
||||
//}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef volatile struct {
|
||||
float q0;
|
||||
float q1;
|
||||
float q2;
|
||||
float q3;
|
||||
} Quaternion;
|
||||
|
||||
|
||||
void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt)
|
||||
{
|
||||
volatile struct V{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} Gravity,Acc,Gyro,AccGravity;
|
||||
|
||||
static struct V GyroIntegError = {0};
|
||||
static float KpDef = 0.8f ;
|
||||
static float KiDef = 0.0003f;
|
||||
static Quaternion NumQ = {1, 0, 0, 0};
|
||||
float q0_t,q1_t,q2_t,q3_t;
|
||||
//float NormAcc;
|
||||
float NormQuat;
|
||||
float HalfTime = dt * 0.5f;
|
||||
|
||||
|
||||
|
||||
// 提取等效旋转矩阵中的重力分量
|
||||
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
|
||||
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
|
||||
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);
|
||||
// 加速度归一化
|
||||
NormAcc = Q_rsqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
|
||||
|
||||
Acc.x = pMpu->accX * NormAcc;
|
||||
Acc.y = pMpu->accY * NormAcc;
|
||||
Acc.z = pMpu->accZ * NormAcc;
|
||||
//向量差乘得出的值
|
||||
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
|
||||
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
|
||||
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
|
||||
//再做加速度积分补偿角速度的补偿值
|
||||
GyroIntegError.x += AccGravity.x * KiDef;
|
||||
GyroIntegError.y += AccGravity.y * KiDef;
|
||||
GyroIntegError.z += AccGravity.z * KiDef;
|
||||
//角速度融合加速度积分补偿值
|
||||
Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x;//弧度制
|
||||
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;
|
||||
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z + GyroIntegError.z;
|
||||
// 一阶龙格库塔法, 更新四元数
|
||||
|
||||
q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
|
||||
q1_t = ( NumQ.q0*Gyro.x - NumQ.q3*Gyro.y + NumQ.q2*Gyro.z) * HalfTime;
|
||||
q2_t = ( NumQ.q3*Gyro.x + NumQ.q0*Gyro.y - NumQ.q1*Gyro.z) * HalfTime;
|
||||
q3_t = (-NumQ.q2*Gyro.x + NumQ.q1*Gyro.y + NumQ.q0*Gyro.z) * HalfTime;
|
||||
|
||||
NumQ.q0 += q0_t;
|
||||
NumQ.q1 += q1_t;
|
||||
NumQ.q2 += q2_t;
|
||||
NumQ.q3 += q3_t;
|
||||
// 四元数归一化
|
||||
NormQuat = Q_rsqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
|
||||
NumQ.q0 *= NormQuat;
|
||||
NumQ.q1 *= NormQuat;
|
||||
NumQ.q2 *= NormQuat;
|
||||
NumQ.q3 *= NormQuat;
|
||||
|
||||
|
||||
// 四元数转欧拉角
|
||||
{
|
||||
|
||||
#ifdef YAW_GYRO
|
||||
*(float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA; //yaw
|
||||
#else
|
||||
float yaw_G = pMpu->gyroZ * Gyro_G;
|
||||
if((yaw_G > 3.0f) || (yaw_G < -3.0f)) //数据太小可以认为是干扰，不是偏航动作
|
||||
{
|
||||
pAngE->yaw += yaw_G * dt;
|
||||
}
|
||||
#endif
|
||||
pAngE->pitch = asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;
|
||||
|
||||
pAngE->roll = atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA; //PITCH
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/***************************************************END OF FILE***************************************************/
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
#ifndef __IMU_H
|
||||
#define __IMU_H
|
||||
|
||||
|
||||
#include "data.h"
|
||||
|
||||
|
||||
//extern float GetNormAccz(void);
|
||||
extern void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt);
|
||||
|
||||
|
||||
#endif
|
||||
|
|
@ -1,6 +1,41 @@
|
|||
#include "M451Series.h"
|
||||
#include "data.h"
|
||||
#include "mpu.h"
|
||||
#include "imu.h"
|
||||
|
||||
#define PLL_CLOCK 5000000
|
||||
|
||||
|
||||
void Timer0Init()
|
||||
{
|
||||
CLK_EnableModuleClock(TMR0_MODULE);
|
||||
TIMER_Open(TIMER0,TIMER_PERIODIC_MODE,200);
|
||||
TIMER_EnableCapture(TIMER0,TIMER_CAPTURE_FREE_COUNTING_MODE,TIMER_CAPTURE_FALLING_AND_RISING_EDGE);
|
||||
TIMER_EnableInt(TIMER0);
|
||||
TIMER_Start(TIMER0);
|
||||
SYS_UnlockReg();
|
||||
NVIC_EnableIRQ(TMR0_IRQn);
|
||||
SYS_LockReg();
|
||||
}
|
||||
|
||||
|
||||
void TMR0_IRQHandler()
|
||||
{
|
||||
TIMER_ClearIntFlag(TIMER0);
|
||||
|
||||
static uint8_t cnt_3ms = 0;
|
||||
static uint8_t cnt_6ms = 0;
|
||||
cnt_3ms++;
|
||||
cnt_6ms++;
|
||||
if (cnt_6ms >= 2){
|
||||
cnt_6ms = 0;
|
||||
MpuGetData();
|
||||
GetAngle(&MPU6050UnFiltered,&Angle,0.010f);
|
||||
}
|
||||
else{
|
||||
}
|
||||
|
||||
}
|
||||
void HalInit(){
|
||||
SYS_UnlockReg();
|
||||
|
||||
|
|
@ -41,7 +76,7 @@ void HalInit(){
|
|||
|
||||
SYS->GPD_MFPL &= ~SYS_GPD_MFPL_PD5MFP_Msk;
|
||||
SYS->GPD_MFPL |= SYS_GPD_MFPL_PD5MFP_I2C0_SCL;
|
||||
I2C_Open(I2C0,100000);
|
||||
I2C_Open(I2C0,50000);
|
||||
|
||||
printf("I2C clock %d Hz\n", I2C_GetBusClockFreq(I2C0));
|
||||
|
||||
|
|
@ -60,5 +95,9 @@ void HalInit(){
|
|||
int main(){
|
||||
HalInit();
|
||||
MpuInit();
|
||||
|
||||
|
||||
Timer0Init();
|
||||
while(1){
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -1,55 +1,60 @@
|
|||
#include "data.h"
|
||||
#include "hal.h"
|
||||
#include "mpu.h"
|
||||
|
||||
|
||||
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
|
||||
#define CONFIGL 0x1A //低通滤波频率，典型值：0x06(5Hz)
|
||||
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
|
||||
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
|
||||
#define ACCEL_ADDRESS 0x3B
|
||||
#define ACCEL_XOUT_H 0x3B
|
||||
#define ACCEL_XOUT_L 0x3C
|
||||
#define ACCEL_YOUT_H 0x3D
|
||||
#define ACCEL_YOUT_L 0x3E
|
||||
#define ACCEL_ZOUT_H 0x3F
|
||||
#define ACCEL_ZOUT_L 0x40
|
||||
#define TEMP_OUT_H 0x41
|
||||
#define TEMP_OUT_L 0x42
|
||||
#define GYRO_XOUT_H 0x43
|
||||
#define GYRO_ADDRESS 0x43
|
||||
#define GYRO_XOUT_L 0x44
|
||||
#define GYRO_YOUT_H 0x45
|
||||
#define GYRO_YOUT_L 0x46
|
||||
#define GYRO_ZOUT_H 0x47
|
||||
#define GYRO_ZOUT_L 0x48
|
||||
#define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
|
||||
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
|
||||
#define MPU6050_PRODUCT_ID 0x68
|
||||
#define MPU6052C_PRODUCT_ID 0x72
|
||||
#define MPU9250_PRODUCT_ID 0x71 //
|
||||
//#define MPU6050_is_DRY() GPIO_ReadOutBit(HT_GPIOC, GPIO_PIN_0)//IRQ主机数据输入
|
||||
#ifdef USE_I2C_HARDWARE
|
||||
|
||||
#define MPU6050_ADDRESS 0xD0//0x68
|
||||
#else
|
||||
#define MPU6050_ADDRESS 0xD0 //IIC写入时的地址字节数据，+1为读取
|
||||
#define MPU6050_ADDRESS 0xD0 //IIC写入时的地å<C2B0>€å—节数æ<C2B0>®ï¼?1为读å<C2BB>?
|
||||
|
||||
#endif
|
||||
|
||||
int i2c0Lock = 0;
|
||||
|
||||
|
||||
#define Gyro_Read() MPUReadBuf(MPU6050_ADDRESS,0x3b,buffer,6)
|
||||
#define Acc_Read() MPUReadBuf(MPU6050_ADDRESS, 0x43,&buffer[6],6)
|
||||
#define Mag_Read() MPUReadBuf(AK8963_I2C_ADDR,0x03,&buffer[12],6)
|
||||
|
||||
void delay_ms(int x){
|
||||
|
||||
for(x = 0; x < 1000; x++){
|
||||
volatile int z = 0;
|
||||
for (z = 0; z < 100;z++){
|
||||
|
||||
int z = 0;
|
||||
for (z = 0; z < 1020;){
|
||||
z ++ ;
|
||||
}
|
||||
}
|
||||
}
|
||||
void I2C0_LCK (){
|
||||
if (i2c0Lock == 0){
|
||||
i2c0Lock = 1;
|
||||
}else{
|
||||
while(i2c0Lock == 0){
|
||||
i2c0Lock = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void I2C0_UNLOCK(){
|
||||
if (i2c0Lock == 1){
|
||||
i2c0Lock = 0;
|
||||
}else{
|
||||
while(i2c0Lock == 1){
|
||||
i2c0Lock = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int16_t MpuOffset[6] = {0};
|
||||
|
||||
_st_AngE Angle;
|
||||
_st_Mpu MPU6050; //MPU6050原始数据
|
||||
_st_Mpu MPU6050UnFiltered; //MPU6050原始数æ<C2B0>®
|
||||
|
||||
_st_Mpu_Angle MPU6050Angle;
|
||||
static volatile int16_t *pMpu = (int16_t *)&MPU6050;
|
||||
static volatile int16_t *pMpuUnFilter = (int16_t *)&MPU6050UnFiltered;
|
||||
|
||||
|
||||
|
||||
|
|
@ -60,7 +65,7 @@ static volatile int16_t *pMpu = (int16_t *)&MPU6050;
|
|||
*****************************************************************************************/
|
||||
int8_t mpu6050_rest(void)
|
||||
{
|
||||
if(MPUWriteReg( PWR_MGMT_1, 0x80) == FAILED)
|
||||
if(MPUWriteReg(MPU6050_ADDRESS,PWR_MGMT_1, 0x80) == FAILED)
|
||||
return FAILED; //复位
|
||||
//delay_ms(20);
|
||||
return SUCCESS;
|
||||
|
|
@ -70,26 +75,60 @@ int8_t mpu6050_rest(void)
|
|||
*@brief
|
||||
*@param[in]
|
||||
*****************************************************************************************/
|
||||
int8_t MpuInit(void) //初始化
|
||||
int8_t MpuInit(void) //åˆ<EFBFBD>始åŒ?
|
||||
{
|
||||
uint8_t date = SUCCESS;
|
||||
do
|
||||
{
|
||||
date = MPUWriteReg( PWR_MGMT_1, 0x80); //复位
|
||||
|
||||
|
||||
date = MPUWriteReg(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // reset
|
||||
delay_ms(30);
|
||||
date += MPUWriteReg( SMPLRT_DIV, 0x02); //陀螺仪采样率，0x00(500Hz)
|
||||
date += MPUWriteReg( PWR_MGMT_1, 0x03); //设置设备时钟源，陀螺仪Z轴
|
||||
date += MPUWriteReg( CONFIGL, 0x03); //低通滤波频率，0x03(42Hz)
|
||||
date += MPUWriteReg( GYRO_CONFIG, 0x18);//+-2000deg/s
|
||||
date += MPUWriteReg( ACCEL_CONFIG, 0x09);//+-4G
|
||||
date += MPUWriteReg(MPU6050_ADDRESS,SMPLRT_DIV, 0x02); //sample rate
|
||||
date += MPUWriteReg( MPU6050_ADDRESS,PWR_MGMT_1, 0x03); // clock source
|
||||
date += MPUWriteReg( MPU6050_ADDRESS,CONFIGL, 0x03); // lpf
|
||||
|
||||
date += MPUWriteReg( MPU6050_ADDRESS,GYRO_CONFIG, 0x18);//+-2000deg/s
|
||||
date += MPUWriteReg( MPU6050_ADDRESS,ACCEL_CONFIG, 0x09);//+-4G
|
||||
delay_ms(30);
|
||||
delay_ms(30);
|
||||
MPUWriteReg(MPU6050_ADDRESS,USER_CTRL,0x00); //
|
||||
delay_ms(30);
|
||||
delay_ms(30);
|
||||
|
||||
date += MPUWriteReg( MPU6050_ADDRESS,INT_PIN_CFG ,0X02); //bypass mode
|
||||
delay_ms(30);
|
||||
delay_ms(30);
|
||||
delay_ms(30);
|
||||
//MPUWriteReg(AK8963_I2C_ADDR,AK8963_CNTL1, 0x11);
|
||||
delay_ms(30);
|
||||
delay_ms(30);
|
||||
|
||||
delay_ms(30);
|
||||
delay_ms(30);
|
||||
|
||||
date += MPUWriteReg( AK8963_I2C_ADDR,0x0A,0x01); //one-time messure
|
||||
|
||||
date = MPUReadReg(MPU6050_ADDRESS,WHO_AM_I);
|
||||
//if(date!= MPU9250_PRODUCT_ID)
|
||||
//return FAILED;
|
||||
char us = MPUReadReg( AK8963_I2C_ADDR,0);//+-2000deg/s
|
||||
if (us != 0x48){
|
||||
return FAIL;
|
||||
}
|
||||
while(date != SUCCESS);
|
||||
date = MPUReadReg(0x75);
|
||||
if(date!= MPU6050_PRODUCT_ID)
|
||||
return FAILED;
|
||||
else
|
||||
//MpuGetOffset();
|
||||
return SUCCESS;
|
||||
}
|
||||
|
||||
void MpuAngle(){
|
||||
MPU6050Angle.accX = MPU6050.accX *360/32768;
|
||||
MPU6050Angle.accY = MPU6050.accY *360/32768;
|
||||
MPU6050Angle.accZ = MPU6050.accZ *360/32768;
|
||||
|
||||
MPU6050Angle.gyroX = MPU6050.gyroX*3.1415926/180;
|
||||
MPU6050Angle.gyroY = MPU6050.gyroY*3.1415926/180;
|
||||
MPU6050Angle.gyroZ = MPU6050.gyroZ*3.1415926/180;
|
||||
|
||||
}
|
||||
|
||||
void MpuMag(){
|
||||
|
||||
}
|
||||
/****************************************************************************************
|
||||
*@brief
|
||||
|
|
@ -97,36 +136,47 @@ int8_t MpuInit(void) //初始化
|
|||
*@param[in]
|
||||
*****************************************************************************************/
|
||||
|
||||
#define Gyro_Read() MPUReadBuf( 0X3B,buffer,6)
|
||||
#define Acc_Read() MPUReadBuf( 0x43,&buffer[6],6)
|
||||
void kalman_1(struct _1_ekf_filter *ekf,float input) //一维å<C2B4>¡å°”曼
|
||||
{
|
||||
ekf->Now_P = ekf->LastP + ekf->Q;
|
||||
ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
|
||||
ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
|
||||
ekf->LastP = (1-ekf->Kg) * ekf->Now_P ;
|
||||
}
|
||||
|
||||
void MpuGetData(void) //读取陀螺仪数据加滤波
|
||||
|
||||
void MpuGetData(void) //读å<C2BB>–陀螺仪数æ<C2B0>®åŠ 滤æ³?
|
||||
{
|
||||
uint8_t i;
|
||||
int8_t buffer[12];
|
||||
int8_t buffer[18];
|
||||
|
||||
MPUWriteReg( AK8963_I2C_ADDR,0x0A,0x01);
|
||||
//Mag_Read();
|
||||
|
||||
Gyro_Read();
|
||||
Acc_Read();
|
||||
|
||||
for(i=0;i<6;i++)
|
||||
Acc_Read();
|
||||
MpuAngle();
|
||||
for(i = 0; i < 9;i++)
|
||||
{
|
||||
pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];
|
||||
pMpu[i] = (((int16_t)buffer[2*i] *256)| buffer[2*i + 1]&0xff)-MpuOffset[i];
|
||||
pMpuUnFilter[i] = (((int16_t)buffer[2*i] *256)| buffer[2*i + 1]&0xff)-MpuOffset[i];
|
||||
/*
|
||||
if(i < 3)
|
||||
{
|
||||
{
|
||||
//static struct _1_ekf_filter ekf[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};
|
||||
//kalman_1(&ekf[i],(float)pMpu[i]); //一维卡尔曼
|
||||
//pMpu[i] = (int16_t)ekf[i].out;
|
||||
static struct _1_ekf_filter ekf[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};
|
||||
kalman_1(&ekf[i],(float)pMpu[i]); //一维å<C2B4>¡å°”曼
|
||||
pMpu[i] = (int16_t)ekf[i].out;
|
||||
}
|
||||
}
|
||||
if(i > 2)
|
||||
{
|
||||
uint8_t k=i-3;
|
||||
const float factor = 0.15f; //滤波因素
|
||||
static float tBuff[3];
|
||||
}
|
||||
if(i > 2)
|
||||
{
|
||||
uint8_t k=i-3;
|
||||
const float factor = 0.15f; //æ»¤æ³¢å› ç´
|
||||
static float tBuff[3];
|
||||
|
||||
pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
|
||||
}
|
||||
pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
|
||||
}*/
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,83 @@
|
|||
#ifndef __MPU__
|
||||
#define __MPU__
|
||||
#include "data.h"
|
||||
|
||||
|
||||
|
||||
// Read-only Reg
|
||||
#define AK8963_I2C_ADDR 0x18 // slave address for the AK8963
|
||||
#define AK8963_DEVICE_ID 0x48
|
||||
#define AK8963_WIA 0x00
|
||||
#define AK8963_INFO 0x01
|
||||
#define AK8963_ST1 0x02 // data ready status bit 0
|
||||
#define AK8963_XOUT_L 0x03 // data
|
||||
#define AK8963_XOUT_H 0x04
|
||||
#define AK8963_YOUT_L 0x05
|
||||
#define AK8963_YOUT_H 0x06
|
||||
#define AK8963_ZOUT_L 0x07
|
||||
#define AK8963_ZOUT_H 0x08
|
||||
#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
|
||||
#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
|
||||
#define AK8963_ASTC 0x0C // Self test control
|
||||
#define AK8963_I2CDIS 0x0F // I2C disable
|
||||
#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
|
||||
#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
|
||||
#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
|
||||
|
||||
// Write/Read Reg
|
||||
#define AK8963_CNTL1 0x0A
|
||||
#define AK8963_CNTL2 0x0B
|
||||
#define AK8963_ASTC 0x0C
|
||||
#define AK8963_TS1 0x0D
|
||||
#define AK8963_TS2 0x0E
|
||||
#define AK8963_I2CDIS 0x0F
|
||||
|
||||
|
||||
// Read-only Reg ( ROM )
|
||||
#define AK8963_ASAX 0x10
|
||||
#define AK8963_ASAY 0x11
|
||||
#define AK8963_ASAZ 0x12
|
||||
|
||||
#define SMPLRT_DIV 0x19 //é™€èžºä»ªé‡‡æ ·çŽ‡ï¼Œå…¸åž‹å€¼ï¼š0x07(125Hz)
|
||||
#define CONFIGL 0x1A //低通滤波频率，典型值：0x06(5Hz)
|
||||
#define GYRO_CONFIG 0x1B //陀螺仪自检å<E282AC>Šæµ‹é‡<C3A9>范围，典型值：0x18(ä¸<C3A4>自检ï¼?000deg/s)
|
||||
#define ACCEL_CONFIG 0x1C //åŠ é€Ÿè®¡è‡ªæ£€ã€<C3A3>测é‡<C3A9>范围å<C2B4>Šé«˜é€šæ»¤æ³¢é¢‘率，典型值：0x01(ä¸<C3A4>自检ï¼?Gï¼?Hz)
|
||||
#define ACCEL_CONFIG2 0X1D //åŠ é€Ÿåº¦è®¡ä½Žé€šæ»¤æ³¢å™¨ 0x06 5hz
|
||||
|
||||
#define PWR_MGMT_1 0X6B//电æº<C3A6>管ç<C2A1>†1 典型值为0x00
|
||||
|
||||
#define INT_PIN_CFG 0x37
|
||||
#define ACCEL_ADDRESS 0x3B
|
||||
#define ACCEL_XOUT_H 0x3B
|
||||
#define ACCEL_XOUT_L 0x3C
|
||||
#define ACCEL_YOUT_H 0x3D
|
||||
#define ACCEL_YOUT_L 0x3E
|
||||
#define ACCEL_ZOUT_H 0x3F
|
||||
#define ACCEL_ZOUT_L 0x40
|
||||
#define TEMP_OUT_H 0x41
|
||||
#define TEMP_OUT_L 0x42
|
||||
#define GYRO_XOUT_H 0x43
|
||||
#define GYRO_ADDRESS 0x43
|
||||
#define GYRO_XOUT_L 0x44
|
||||
#define GYRO_YOUT_H 0x45
|
||||
#define GYRO_YOUT_L 0x46
|
||||
#define GYRO_ZOUT_H 0x47
|
||||
#define GYRO_ZOUT_L 0x48
|
||||
#define WHO_AM_I 0x75 //IIC地å<C2B0>€å¯„å˜å™?默认数å€?x68，å<C592>ªè¯?
|
||||
#define MPU6050_PRODUCT_ID 0x68
|
||||
#define MPU6052C_PRODUCT_ID 0x72
|
||||
#define MPU9250_PRODUCT_ID 0x71 //
|
||||
#define I2C_SLV0_ADDR 0x25
|
||||
#define EXT_SENS_DATA_00 0x49
|
||||
#define I2C_SLV0_CTRL 0x27
|
||||
#define I2C_SLV0_DO 0x64
|
||||
#define USER_CTRL 0x6A
|
||||
|
||||
extern _st_Mpu MPU6050; //MPU6050原始数æ<C2B0>®
|
||||
extern _st_AngE Angle;
|
||||
extern _st_AngE Angle;
|
||||
|
||||
int8_t MpuInit(void); //åˆ<C3A5>始åŒ?
|
||||
void MpuGetData(void); //读å<C2BB>–陀螺仪数æ<C2B0>®åŠ 滤æ³?
|
||||
|
||||
#endif
|
||||
|
|
@ -28,7 +28,7 @@
|
|||
<TargetOption>
|
||||
<CLKADS>12000000</CLKADS>
|
||||
<OPTTT>
|
||||
<gFlags>0</gFlags>
|
||||
<gFlags>1</gFlags>
|
||||
<BeepAtEnd>1</BeepAtEnd>
|
||||
<RunSim>0</RunSim>
|
||||
<RunTarget>1</RunTarget>
|
||||
|
|
@ -103,7 +103,7 @@
|
|||
<bEvRecOn>1</bEvRecOn>
|
||||
<bSchkAxf>0</bSchkAxf>
|
||||
<bTchkAxf>0</bTchkAxf>
|
||||
<nTsel>0</nTsel>
|
||||
<nTsel>8</nTsel>
|
||||
<sDll></sDll>
|
||||
<sDllPa></sDllPa>
|
||||
<sDlgDll></sDlgDll>
|
||||
|
|
@ -114,9 +114,29 @@
|
|||
<tDlgDll></tDlgDll>
|
||||
<tDlgPa></tDlgPa>
|
||||
<tIfile></tIfile>
|
||||
<pMon>BIN\UL2CM3.DLL</pMon>
|
||||
<pMon>NULink\Nu_Link.dll</pMon>
|
||||
</DebugOpt>
|
||||
<TargetDriverDllRegistry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>ARMRTXEVENTFLAGS</Key>
|
||||
<Name>-L70 -Z18 -C0 -M0 -T1</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>DLGTARM</Key>
|
||||
<Name>(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)</Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>ARMDBGFLAGS</Key>
|
||||
<Name></Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>Nu_Link</Key>
|
||||
<Name></Name>
|
||||
</SetRegEntry>
|
||||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<Key>UL2CM3</Key>
|
||||
|
|
@ -124,18 +144,50 @@
|
|||
</SetRegEntry>
|
||||
</TargetDriverDllRegistry>
|
||||
<Breakpoint/>
|
||||
<WatchWindow1>
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>MPU6050</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>1</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>MPU6050Angle</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>2</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>buffer</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>us</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>Angle</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>MpuOffset</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
</Tracepoint>
|
||||
<DebugFlag>
|
||||
<trace>0</trace>
|
||||
<periodic>1</periodic>
|
||||
<aLwin>0</aLwin>
|
||||
<aLwin>1</aLwin>
|
||||
<aCover>0</aCover>
|
||||
<aSer1>0</aSer1>
|
||||
<aSer2>0</aSer2>
|
||||
<aPa>0</aPa>
|
||||
<viewmode>0</viewmode>
|
||||
<viewmode>1</viewmode>
|
||||
<vrSel>0</vrSel>
|
||||
<aSym>0</aSym>
|
||||
<aTbox>0</aTbox>
|
||||
|
|
@ -166,6 +218,12 @@
|
|||
<pszMrulep></pszMrulep>
|
||||
<pSingCmdsp></pSingCmdsp>
|
||||
<pMultCmdsp></pMultCmdsp>
|
||||
<SystemViewers>
|
||||
<Entry>
|
||||
<Name>System Viewer\I2C0</Name>
|
||||
<WinId>35905</WinId>
|
||||
</Entry>
|
||||
</SystemViewers>
|
||||
</TargetOption>
|
||||
</Target>
|
||||
|
||||
|
|
@ -223,6 +281,30 @@
|
|||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>1</GroupNumber>
|
||||
<FileNumber>5</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\imu.c</PathWithFileName>
|
||||
<FilenameWithoutPath>imu.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>1</GroupNumber>
|
||||
<FileNumber>6</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\myMath.c</PathWithFileName>
|
||||
<FilenameWithoutPath>myMath.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
</File>
|
||||
</Group>
|
||||
|
||||
<Group>
|
||||
|
|
|
|||
|
|
@ -133,12 +133,12 @@
|
|||
<UseExternalTool>0</UseExternalTool>
|
||||
<RunIndependent>0</RunIndependent>
|
||||
<UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
|
||||
<Capability>0</Capability>
|
||||
<DriverSelection>-1</DriverSelection>
|
||||
<Capability>1</Capability>
|
||||
<DriverSelection>4096</DriverSelection>
|
||||
</Flash1>
|
||||
<bUseTDR>1</bUseTDR>
|
||||
<Flash2>BIN\UL2CM3.DLL</Flash2>
|
||||
<Flash3></Flash3>
|
||||
<Flash3>"" ()</Flash3>
|
||||
<Flash4></Flash4>
|
||||
<pFcarmOut></pFcarmOut>
|
||||
<pFcarmGrp></pFcarmGrp>
|
||||
|
|
@ -257,17 +257,17 @@
|
|||
<Size>0x0</Size>
|
||||
</XRAM>
|
||||
<OCR_RVCT1>
|
||||
<Type>0</Type>
|
||||
<Type>1</Type>
|
||||
<StartAddress>0x0</StartAddress>
|
||||
<Size>0x0</Size>
|
||||
</OCR_RVCT1>
|
||||
<OCR_RVCT2>
|
||||
<Type>0</Type>
|
||||
<Type>1</Type>
|
||||
<StartAddress>0x0</StartAddress>
|
||||
<Size>0x0</Size>
|
||||
</OCR_RVCT2>
|
||||
<OCR_RVCT3>
|
||||
<Type>0</Type>
|
||||
<Type>1</Type>
|
||||
<StartAddress>0x0</StartAddress>
|
||||
<Size>0x0</Size>
|
||||
</OCR_RVCT3>
|
||||
|
|
@ -277,7 +277,7 @@
|
|||
<Size>0x40000</Size>
|
||||
</OCR_RVCT4>
|
||||
<OCR_RVCT5>
|
||||
<Type>0</Type>
|
||||
<Type>1</Type>
|
||||
<StartAddress>0x0</StartAddress>
|
||||
<Size>0x0</Size>
|
||||
</OCR_RVCT5>
|
||||
|
|
@ -401,6 +401,16 @@
|
|||
<FileType>5</FileType>
|
||||
<FilePath>.\data.h</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>imu.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>.\imu.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>myMath.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>.\myMath.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
|
@ -459,6 +469,12 @@
|
|||
<targetInfo name="Target 1"/>
|
||||
</targetInfos>
|
||||
</component>
|
||||
<component Cclass="Device" Cgroup="Driver" Csub="Timer" Cvendor="Nuvoton" Cversion="3.01.001" condition="M4NuMicro M451 Device">
|
||||
<package name="NuMicro_DFP" schemaVersion="1.2" url="http://www.nuvoton.com/hq/enu/Documents/KEILSoftwarePack" vendor="Nuvoton" version="1.2.0"/>
|
||||
<targetInfos>
|
||||
<targetInfo name="Target 1"/>
|
||||
</targetInfos>
|
||||
</component>
|
||||
<component Cclass="Device" Cgroup="Startup" Cvendor="Nuvoton" Cversion="3.01.001" condition="M4NuMicro M451 Device">
|
||||
<package name="NuMicro_DFP" schemaVersion="1.2" url="http://www.nuvoton.com/hq/enu/Documents/KEILSoftwarePack" vendor="Nuvoton" version="1.2.0"/>
|
||||
<targetInfos>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,360 @@
|
|||
//========================================================================
|
||||
// 爱好者电子工作室-淘宝 https://devotee.taobao.com/
|
||||
// STM32四轴爱好者QQ群: 810149456
|
||||
// 作者：小刘
|
||||
// 电话:13728698082
|
||||
// 邮箱:1042763631@qq.com
|
||||
// 日期：2018.05.17
|
||||
// 版本：V1.0
|
||||
//========================================================================
|
||||
//套件购买地址：https://devotee.taobao.com/
|
||||
// 爱好者电子工作室
|
||||
//特此声明：
|
||||
//
|
||||
// 此程序只能用作学习，如用商业用途。必追究责任！
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
#include "data.h"
|
||||
#include "myMath.h"
|
||||
#include <math.h>
|
||||
|
||||
|
||||
const float M_PI = 3.1415926535;
|
||||
const float RtA = 57.2957795f;
|
||||
const float AtR = 0.0174532925f;
|
||||
const float Gyro_G = 0.03051756f*2; //陀螺仪初始化量程+-2000度每秒于1 / (65536 / 4000) = 0.03051756*2
|
||||
const float Gyro_Gr = 0.0005326f*2; //面计算度每秒,转换弧度每秒则 2*0.03051756 * 0.0174533f = 0.0005326*2
|
||||
////in -+500
|
||||
////out exp -+500
|
||||
//int16_t Math_AngelEXP(int16_t in){
|
||||
// int16_t tmp2 ,tmp,value;
|
||||
// tmp = Math_min(Math_abs(in),500); //[0 , +500]
|
||||
// tmp2 = tmp/100;
|
||||
// value = Angel_EXP[tmp2] + (tmp-tmp2*100) * (Angel_EXP[tmp2+1]-Angel_EXP[tmp2]) / 100;
|
||||
// if(in<0)value = -value;
|
||||
// return value;
|
||||
//}
|
||||
|
||||
////油门输入转换。
|
||||
//int16_t Math_ThrEXP(int16_t RCThr){
|
||||
// int16_t tmp2 ,tmp,value;
|
||||
// if(RCThr <1000)return RCThr; //低于最小值，直接输出，不做指数变换
|
||||
// tmp = Math_Constrain(RCThr,1000,2000);
|
||||
// tmp = (unsigned int)(tmp-1000)*1000/(2000-1000); // [1000;2000] -> [0;1000]
|
||||
// tmp2 = tmp/100;
|
||||
// value = ThrottleEXP[tmp2] + (tmp-tmp2*100) * (ThrottleEXP[tmp2+1]-ThrottleEXP[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
|
||||
// return value;
|
||||
//}
|
||||
|
||||
|
||||
/*====================================================================================================*/
|
||||
/*====================================================================================================*
|
||||
**函数 : Q_rsqrt
|
||||
**功能 : 快速计算 三角函数
|
||||
**输入 : number
|
||||
**输出 : 结果
|
||||
**备注 : None
|
||||
**====================================================================================================*/
|
||||
/*====================================================================================================*/
|
||||
//逼近法；线性拟合
|
||||
//Q (4/M_PI x - 4/M_PI^2 x^2) + P (4/M_PI x - 4/M_PI^2 x^2)^2
|
||||
#ifndef TAPYOR
|
||||
float sine(float x) // (-M_PI , M_PI) ???? 0.0005
|
||||
{
|
||||
const float Q = 0.775;
|
||||
const float P = 0.225;
|
||||
const float B = 4 / M_PI;
|
||||
const float C = -4 /(M_PI*M_PI);
|
||||
float y = B * x + C * x * fabs(x);
|
||||
return (Q * y + P * y * fabs(y));
|
||||
}
|
||||
#else
|
||||
//4级泰勒公式法 在PI出会出现0.7的最大误差
|
||||
//sinx= x- x^3/3! + x^5/5! - x^7/7!+ x^9/9! . =?(-1)^n x^(2n+1)/(2n+1)!
|
||||
float sine(float x)
|
||||
{
|
||||
float t=x;
|
||||
float result = x;
|
||||
float X2 = x*x;
|
||||
uint8_t cnt = 1;
|
||||
|
||||
do
|
||||
{
|
||||
t=-t;
|
||||
t *= X2;
|
||||
result += t/((cnt<<1)+1);
|
||||
cnt++;
|
||||
} while(cnt<5);//6阶
|
||||
|
||||
return result;
|
||||
}
|
||||
#endif
|
||||
//http://wenku.baidu.com/link?url=jUswZ3G2z26IUS72IkeZrizc5V9VdR1sTF8xGCOHPFW0P70bGjjm5zhNxvRT36X31TMoFf6S-9lMoIkK4pPwExAaEZGtRpWggdQAzpg3Fsu
|
||||
//cos(x)=sin(M_PI/2+x)=sin(M_PI/2-x)
|
||||
//cos(x-M_PI/2)=sin(x)
|
||||
float cosine(float x)
|
||||
{
|
||||
return sine(x+M_PI/2);//奇变偶不变，符号看象限
|
||||
}
|
||||
|
||||
//反正切麦克劳林展开式 阶数越高，值越准确 70°以内是准确的
|
||||
//http://www.zybang.com/question/246f9997776f7d5cc636b10aff27a1cb.html
|
||||
float arctan(float x) // (-1 , +1) 6? ?? 0.002958
|
||||
{
|
||||
float t = x;
|
||||
float result = 0;
|
||||
float X2 = x * x;
|
||||
unsigned char cnt = 1;
|
||||
do
|
||||
{
|
||||
result += t / ((cnt << 1) - 1);
|
||||
t = -t;
|
||||
t *= X2;
|
||||
cnt++;
|
||||
}while(cnt <= 6);//5??
|
||||
return result;
|
||||
}
|
||||
|
||||
//反正弦麦克劳林展开式 -1<x<+1 42°以内是准确的
|
||||
//http://xuxzmail.blog.163.com/blog/static/25131916200971794014536/
|
||||
const float PI_2 = 1.570796f;
|
||||
float arcsin(float x) //(-1 , +1) ? 0 ???? 6? ??0.005
|
||||
{
|
||||
float d=1;
|
||||
float t=x;
|
||||
unsigned char cnt = 1;
|
||||
float result = 0;
|
||||
float X2 = x*x;
|
||||
|
||||
if (x >= 1.0f)
|
||||
{
|
||||
return PI_2;
|
||||
}
|
||||
if (x <= -1.0f)
|
||||
{
|
||||
return -PI_2;
|
||||
}
|
||||
do
|
||||
{
|
||||
result += t / (d * ((cnt << 1) - 1));
|
||||
t *= X2 * ((cnt << 1) - 1);//
|
||||
d *= (cnt << 1);//2 4 6 8 10 ...
|
||||
cnt++;
|
||||
}while(cnt <= 6);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
//保证输入值是有效的
|
||||
float safe_asin(float v)
|
||||
{
|
||||
if (isnan(v)) {
|
||||
return 0.0;
|
||||
}
|
||||
if (v >= 1.0f) {
|
||||
return M_PI/2;
|
||||
}
|
||||
if (v <= -1.0f) {
|
||||
return -M_PI/2;
|
||||
}
|
||||
return asinf(v);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*====================================================================================================*/
|
||||
/*====================================================================================================*
|
||||
**函数 : Q_rsqrt
|
||||
**功能 : 快速计算 1/Sqrt(x)
|
||||
**输入 : number
|
||||
**输出 : 结果
|
||||
**备注 : None
|
||||
**====================================================================================================*/
|
||||
/*====================================================================================================*/
|
||||
float Q_rsqrt(float number)
|
||||
{
|
||||
long i;
|
||||
float x2, y;
|
||||
const float threehalfs = 1.5F;
|
||||
|
||||
x2 = number * 0.5F;
|
||||
y = number;
|
||||
i = * ( long * ) &y;
|
||||
i = 0x5f3759df - ( i >> 1 );
|
||||
y = * ( float * ) &i;
|
||||
y = y * ( threehalfs - ( x2 * y * y ) ); // 1st iteration （第一次牛顿迭代）
|
||||
return y;
|
||||
}
|
||||
|
||||
/**************************实现函数********************************************
|
||||
*函数原型: array_astrict_lower(int16_t *array,int16_t value)
|
||||
*功 能: 对数组下限限制
|
||||
输入参数： *array 目标数组指针
|
||||
* value
|
||||
输出参数： 无
|
||||
*******************************************************************************/
|
||||
void array_astrict(int16_t *array,int16_t lower,int16_t upper)
|
||||
{
|
||||
int16_t length = sizeof(array);
|
||||
uint16_t i = 0;
|
||||
for(i=0;i<length;i++)
|
||||
{
|
||||
if(*(array+i)<lower) *(array+i) = lower;
|
||||
else if(*(array+i)>upper) *(array+i) = upper;
|
||||
}
|
||||
}
|
||||
|
||||
/**************************实现函数********************************************
|
||||
*函数原型: array_assign(int16_t *array,int16_t value)
|
||||
*功 能: 对数组赋值
|
||||
输入参数： *array 目标数组指针
|
||||
* value
|
||||
输出参数： 无
|
||||
*******************************************************************************/
|
||||
void array_assign(int16_t *array,int16_t value)
|
||||
{
|
||||
uint16_t length = sizeof(array);
|
||||
uint16_t i=0;
|
||||
for(i=0;i<length;i++)
|
||||
{
|
||||
*(array+i) = value;
|
||||
}
|
||||
}
|
||||
|
||||
/**************************实现函数********************************************
|
||||
*函数原型: data_limit(float data,flaot toplimit,float lowerlimit)
|
||||
*功 能: 数据限幅
|
||||
输入参数： data 要操作的数据
|
||||
* toplimit 上限
|
||||
* lowerlimit 下限
|
||||
输出参数： 无
|
||||
*******************************************************************************/
|
||||
float data_limit(float data,float toplimit,float lowerlimit)
|
||||
{
|
||||
if(data > toplimit) data = toplimit;
|
||||
else if(data < lowerlimit) data = lowerlimit;
|
||||
return data;
|
||||
}
|
||||
|
||||
|
||||
/***********************************************
|
||||
* @brief 可变增益自适应参数
|
||||
* @param None
|
||||
* @retval None
|
||||
************************************************/
|
||||
float VariableParameter(float error)
|
||||
{
|
||||
float result = 0;
|
||||
|
||||
if(error < 0)
|
||||
{
|
||||
error = -error;
|
||||
}
|
||||
if(error >0.6f)
|
||||
{
|
||||
error = 0.6f;
|
||||
}
|
||||
result = 1 - 1.667f * error;
|
||||
if(result < 0)
|
||||
{
|
||||
result = 0;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
float middle_3(float input) //3个数取中间的数
|
||||
{
|
||||
|
||||
int a,b,c,t;
|
||||
|
||||
|
||||
if(a<b)
|
||||
|
||||
{
|
||||
|
||||
t=a;a=b;b=t;
|
||||
|
||||
}
|
||||
|
||||
if(b<c)//9 8 7
|
||||
|
||||
{
|
||||
|
||||
t=b;b=c;c=t;
|
||||
|
||||
}
|
||||
|
||||
if(a<b)//9 8 7
|
||||
|
||||
{
|
||||
|
||||
t=a;a=b;b=t;
|
||||
|
||||
}
|
||||
|
||||
return b;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**************************实现函数********************************************
|
||||
*函数原型: rad(double angle)
|
||||
*功 能: 角度转化为弧度
|
||||
输入参数： 角度
|
||||
输出参数： 弧度
|
||||
*******************************************************************************/
|
||||
//float Rad(float angle)
|
||||
//{
|
||||
// return angle * AtR ;
|
||||
//}
|
||||
/**************************实现函数********************************************
|
||||
*函数原型: degree(double rad)
|
||||
*功 能: 弧度转化为角度
|
||||
输入参数： 弧度
|
||||
输出参数： 角度
|
||||
*******************************************************************************/
|
||||
|
||||
|
||||
float my_deathzoom_2(float x,float zoom)
|
||||
{
|
||||
float t;
|
||||
|
||||
if( x> -zoom && x < zoom )
|
||||
{
|
||||
t = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
t = x;
|
||||
}
|
||||
return (t);
|
||||
}
|
||||
|
||||
|
||||
float my_deathzoom(float x,float zoom)
|
||||
{
|
||||
float t;
|
||||
if(x>0)
|
||||
{
|
||||
t = x - zoom;
|
||||
if(t<0)
|
||||
{
|
||||
t = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
t = x + zoom;
|
||||
if(t>0)
|
||||
{
|
||||
t = 0;
|
||||
}
|
||||
}
|
||||
return (t);
|
||||
}
|
||||
/******************* (C) COPYRIGHT 2012 WildFire Team *****END OF FILE************/
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
#ifndef __MY_MATH_H
|
||||
#define __MY_MATH_H
|
||||
|
||||
extern const float M_PI;
|
||||
extern const float AtR;
|
||||
extern const float RtA;
|
||||
extern const float Gyro_G;
|
||||
extern const float Gyro_Gr;
|
||||
#define PI 3.1415926f
|
||||
#define squa( Sq ) (((float)Sq)*((float)Sq))
|
||||
#define absu16( Math_X ) ((Math_X)<0? -(Math_X):(Math_X))
|
||||
#define absFloat( Math_X )((Math_X)<0? -(Math_X):(Math_X))
|
||||
#define min(a, b) ((a) < (b) ? (a) : (b))
|
||||
#define max(a, b) ((a) > (b) ? (a) : (b))
|
||||
#define ABS(x) ((x) > 0 ? (x) : -(x))
|
||||
#define LIMIT( x,min,max ) ( (x) < (min) ? (min) : ( (x) > (max) ? (max) : (x) ) )
|
||||
|
||||
extern float safe_asin(float v);
|
||||
extern float arcsin(float x);
|
||||
extern float arctan(float x);
|
||||
extern float sine(float x);
|
||||
extern float cosine(float x);
|
||||
extern float Q_rsqrt(float number);
|
||||
extern float VariableParameter(float error);
|
||||
#endif /* __Algorithm_math_H */
|
||||
Loading…
Reference in New Issue