|
mpu9250芯片内置磁力计灵敏度工厂校正值参数读取 stm32 arduino
一般情况下读到的?ASAX，ASAY,ASAZ 为163,167,153 这样的固定值，每一片9250芯片都不同，处理工厂校正值后然后再进行地磁椭球校正（见我以前的文章）
通过这两步校正后，航向角yaw的误差在1度内，基本能满足各类应用。
#include "i2c.h" ? //i2c通讯部分
#define AK8963_I2C_ADDR ? ? 0x18 ? //0x0C ? ? //地磁仪地址为0x18
/* Read-only Reg ( ROM ) */ //芯片内置地磁仪校正值
#define AK8963_ASAX ? ? ? ? ? ? ? 0x10
#define AK8963_ASAY ? ? ? ? ? ? ? 0x11
#define AK8963_ASAZ ? ? ? ? ? ? ? 0x12
#define AK8963_CNTL1 ? ? ? ? ? ? 0x0A
#define AK8963_CNTL2 ? ? ? ? ? ? 0x0B
float ?mag_sensadj[3] = {1, 1, 1};
float ?AK8963_ASA[3] = {0};
uint8_t asa[3] = {0};
?
void MPU92_AUX_AK8963_Init( void );
//读取厂家芯片内地磁校正值
void MPU92_AUX_AK8963_Init( void )
{
? uint8_t res;
? Delayms(1);
? Single_Write(AK8963_I2C_ADDR, AK8963_CNTL2, 0x01); ? ?// Reset Device
? Delayms(10);
? Single_Write(AK8963_I2C_ADDR, AK8963_CNTL1, 0x00); ? ?// Power-down mode
? Delayms(1);
? Single_Write(AK8963_I2C_ADDR, AK8963_CNTL1, 0x1F); ? ?// Fuse ROM access mode, Read sensitivity adjustment
? Delayms(10);
? asa[0] = Single_Read(AK8963_I2C_ADDR, AK8963_ASAX);
? Delayms(1);
? asa[1] = Single_Read(AK8963_I2C_ADDR, AK8963_ASAY);
? Delayms(1);
? asa[2] = Single_Read(AK8963_I2C_ADDR, AK8963_ASAZ);
? Delayms(1);
#if ENABLE_DEBUG_LOG
? printf(" >>> AK8963 ASA ? : %02X %02X %02X\r\n", asa[0], asa[1], asa[2]);
#endif
? Single_Write(AK8963_I2C_ADDR, AK8963_CNTL1, 0x00); ? ?// Power-down mode
? Delayms(10);
? Single_Write(AK8963_I2C_ADDR, AK8963_CNTL1, 0x16); ? ?// Continuous measurement mode 2 & 16-bit
? Delayms(10);
? AK8963_ASA[0] = (asa[0] - 128) * 0.5 / 128 + 1;
? AK8963_ASA[1] = (asa[1] - 128) * 0.5 / 128 + 1;
? AK8963_ASA[2] = (asa[2] - 128) * 0.5 / 128 + 1;
? res = Single_Read(AK8963_I2C_ADDR, AK8963_CNTL1) & 0x10;
? switch (res) {
? ? case 0x00: ?mag_sensadj[0] = mag_sensadj[1] = mag_sensadj[2] = 0.6; ? break;
? ? case 0x10: ?mag_sensadj[0] = mag_sensadj[1] = mag_sensadj[2] = 0.15; ?break;
? }
? mag_sensadj[0] *= AK8963_ASA[0];
? mag_sensadj[1] *= AK8963_ASA[1];
? mag_sensadj[2] *= AK8963_ASA[2];
}
/
代码中引用为?
mx=mx*mag_sensadj[0];
my=my*mag_sensadj[1];
mz=mz*mag_sensadj[2];
一般情况下读到的?ASAX，ASAY,ASAZ 为163,167,153 这样的固定值，每一片9250芯片都不同，处理工厂校正值后然后再进行地磁椭球校正（见我以前的文章）
通过这两步校正后，航向角yaw的误差在1度内，基本能满足各类应用。
附件?
i2c.h
#ifndef _I2C_H
#define _I2C_H
typedef enum {FALSE=0,TRUE=1} bool;
void I2C_GPIO_Config(void); ?//mpu9250 输入定义
void I2C_delay(void);?? ?
void delay5ms(void);
bool I2C_Start(void);
void I2C_Stop(void);
void I2C_Ack(void);
void I2C_NoAck(void);
bool I2C_WaitAck(void);
void I2C_SendByte(u8 SendByte); //数据从高位到低位//
unsigned char I2C_RadeByte(void); ?//数据从高位到低位//
bool Single_Write(unsigned char SlaveAddress,unsigned char REG_Address,unsigned char REG_data);
unsigned char Single_Read(unsigned char SlaveAddress,unsigned char REG_Address);
void WWDG_Configuration(void);
void WWDG_IRQHandler(void);
#endif
//
i2c.c
#include "stm32f10x.h"
#include "i2c.h"
char ?test=0; ?? ??? ??? ??? ? //IIC
//************************************
/*模拟IIC端口输出输入定义*/
#define SCL_H ? ? ? ? GPIOB->BSRR = GPIO_Pin_6
#define SCL_L ? ? ? ? GPIOB->BRR ?= GPIO_Pin_6?
? ?
#define SDA_H ? ? ? ? GPIOB->BSRR = GPIO_Pin_7
#define SDA_L ? ? ? ? GPIOB->BRR ?= GPIO_Pin_7
#define SCL_read ? ? ?GPIOB->IDR ?& GPIO_Pin_6
#define SDA_read ? ? ?GPIOB->IDR ?& GPIO_Pin_7
/*******************************************************************************
* Function Name ?: I2C_GPIO_Config
* Description ? ?: Configration Simulation IIC GPIO
* Input ? ? ? ? ?: None?
* Output ? ? ? ? : None
* Return ? ? ? ? : None
****************************************************************************** */
void I2C_GPIO_Config(void) ?//mpu9250 输入定义
{
? GPIO_InitTypeDef ?GPIO_InitStructure;?
?
? GPIO_InitStructure.GPIO_Pin = ?GPIO_Pin_6;
? GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
? GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; ?
? GPIO_Init(GPIOB, &GPIO_InitStructure);
? GPIO_InitStructure.GPIO_Pin = ?GPIO_Pin_7;
? GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
? GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
? GPIO_Init(GPIOB, &GPIO_InitStructure);
}
/*******************************************************************************
* Function Name ?: I2C_delay
* Description ? ?: Simulation IIC Timing series delay
* Input ? ? ? ? ?: None
* Output ? ? ? ? : None
* Return ? ? ? ? : None
****************************************************************************** */
void I2C_delay(void)
{
?? ??? ?
? ?u8 i=10; ?// 30 这里可以优化速度?? ?，经测试最低到5还能写入
? ?while(i)?
? ?{?
? ? ?i--;?
? ?} ?
}
void delay5ms(void)
{
?? ??? ?
? ?int i=1000; ?
? ?while(i)?
? ?{?
? ? ?i--;?
? ?} ?
}
/*******************************************************************************
* Function Name ?: I2C_Start
* Description ? ?: Master Start Simulation IIC Communication
* Input ? ? ? ? ?: None
* Output ? ? ? ? : None
* Return ? ? ? ? : Wheather?? ? Start
****************************************************************************** */
bool I2C_Start(void)
{
?? ?SDA_H;
?? ?SCL_H;
?? ?I2C_delay();
?? ?if(!SDA_read)return FALSE;?? ?//SDA线为低电平则总线忙,退出
?? ?SDA_L;
?? ?I2C_delay();
?? ?if(SDA_read) return FALSE;?? ?//SDA线为高电平则总线出错,退出
?? ?SDA_L;
?? ?I2C_delay();
?? ?return TRUE;
}
/*******************************************************************************
* Function Name ?: I2C_Stop
* Description ? ?: Master Stop Simulation IIC Communication
* Input ? ? ? ? ?: None
* Output ? ? ? ? : None
* Return ? ? ? ? : None
****************************************************************************** */
void I2C_Stop(void)
{
?? ?SCL_L;
?? ?I2C_delay();
?? ?SDA_L;
?? ?I2C_delay();
?? ?SCL_H;
?? ?I2C_delay();
?? ?SDA_H;
?? ?I2C_delay();
}?
/*******************************************************************************
* Function Name ?: I2C_Ack
* Description ? ?: Master Send Acknowledge Single
* Input ? ? ? ? ?: None
* Output ? ? ? ? : None
* Return ? ? ? ? : None
****************************************************************************** */
void I2C_Ack(void)
{?? ?
?? ?SCL_L;
?? ?I2C_delay();
?? ?SDA_L;
?? ?I2C_delay();
?? ?SCL_H;
?? ?I2C_delay();
?? ?SCL_L;
?? ?I2C_delay();
} ??
/*******************************************************************************
* Function Name ?: I2C_NoAck
* Description ? ?: Master Send No Acknowledge Single
* Input ? ? ? ? ?: None
* Output ? ? ? ? : None
* Return ? ? ? ? : None
****************************************************************************** */
void I2C_NoAck(void)
{?? ?
?? ?SCL_L;
?? ?I2C_delay();
?? ?SDA_H;
?? ?I2C_delay();
?? ?SCL_H;
?? ?I2C_delay();
?? ?SCL_L;
?? ?I2C_delay();
}?
/*******************************************************************************
* Function Name ?: I2C_WaitAck
* Description ? ?: Master Reserive Slave Acknowledge Single
* Input ? ? ? ? ?: None
* Output ? ? ? ? : None
* Return ? ? ? ? : Wheather?? ? Reserive Slave Acknowledge Single
****************************************************************************** */
bool I2C_WaitAck(void) ?? ? ?//返回为:=1有ACK,=0无ACK
{
?? ?SCL_L;
?? ?I2C_delay();
?? ?SDA_H;?? ??? ??? ?
?? ?I2C_delay();
?? ?SCL_H;
?? ?I2C_delay();
?? ?if(SDA_read)
?? ?{
? ? ? SCL_L;
?? ? ?I2C_delay();
? ? ? return FALSE;
?? ?}
?? ?SCL_L;
?? ?I2C_delay();
?? ?return TRUE;
}
/*******************************************************************************
* Function Name ?: I2C_SendByte
* Description ? ?: Master Send a Byte to Slave
* Input ? ? ? ? ?: Will Send Date
* Output ? ? ? ? : None
* Return ? ? ? ? : None
****************************************************************************** */
void I2C_SendByte(u8 SendByte) //数据从高位到低位//
{
? ? u8 i=8;
? ? while(i--)
? ? {
? ? ? ? SCL_L;
? ? ? ? I2C_delay();
? ? ? if(SendByte&0x80)
? ? ? ? SDA_H; ?
? ? ? else?
? ? ? ? SDA_L; ??
? ? ? ? SendByte<<=1;
? ? ? ? I2C_delay();
?? ??? ?SCL_H;
? ? ? ? I2C_delay();
? ? }
? ? SCL_L;
} ?
/*******************************************************************************
* Function Name ?: I2C_RadeByte
* Description ? ?: Master Reserive a Byte From Slave
* Input ? ? ? ? ?: None
* Output ? ? ? ? : None
* Return ? ? ? ? : Date From Slave?
****************************************************************************** */
unsigned char I2C_RadeByte(void) ?//数据从高位到低位//
{?
? ? u8 i=8;
? ? u8 ReceiveByte=0;
? ? SDA_H;?? ??? ??? ??? ?
? ? while(i--)
? ? {
? ? ? ReceiveByte<<=1; ? ? ?
? ? ? SCL_L;
? ? ? I2C_delay();
?? ? ?SCL_H;
? ? ? I2C_delay();?? ?
? ? ? if(SDA_read)
? ? ? {
? ? ? ? ReceiveByte|=0x01;
? ? ? }
? ? }
? ? SCL_L;
? ? return ReceiveByte;
}?
//ZRX ? ? ? ? ?
//?????*******************************************
bool Single_Write(unsigned char SlaveAddress,unsigned char REG_Address,unsigned char REG_data)?? ??? ? ? ? //void
{
? ?? ?if(!I2C_Start())return FALSE;
? ? I2C_SendByte(SlaveAddress); ? //发送设备地址+写信号//I2C_SendByte(((REG_Address & 0x0700) >>7) | SlaveAddress & 0xFFFE);//设置高起始地址+器件地址?
? ? if(!I2C_WaitAck()){I2C_Stop(); return FALSE;}
? ? I2C_SendByte(REG_Address ); ? //设置低起始地址 ? ? ?
? ? I2C_WaitAck();?? ?
? ? I2C_SendByte(REG_data);
? ? I2C_WaitAck(); ??
? ? I2C_Stop();?
? ? delay5ms();
? ? return TRUE;
}
//单字节读取*****************************************
unsigned char Single_Read(unsigned char SlaveAddress,unsigned char REG_Address)
{ ? unsigned char REG_data; ? ? ?? ?
?? ?if(!I2C_Start())return FALSE;
? ? I2C_SendByte(SlaveAddress); ? //I2C_SendByte(((REG_Address & 0x0700) >>7) | REG_Address & 0xFFFE);//设置高起始地址+器件地址?
? ? if(!I2C_WaitAck()){I2C_Stop();test=1; return FALSE;}
? ? I2C_SendByte((u8) REG_Address); ? //设置高起始地址+器件地址?
? ? I2C_WaitAck();
? ? I2C_Start();
? ? I2C_SendByte(SlaveAddress+1);
? ? I2C_WaitAck();
?? ?REG_data= I2C_RadeByte();
? ? I2C_NoAck();
? ? I2C_Stop();
? ? //return TRUE;
?? ?return REG_data;
}?? ??? ??? ??? ??? ??? ? ? ??
?/*
********************************************************************************
** 函数名称 ： WWDG_Configuration(void)
** 函数功能 ： 看门狗初始化
** 输 ? ?入?? ?： 无
** 输 ? ?出?? ?： 无
** 返 ? ?回?? ?： 无
********************************************************************************
*/
void WWDG_Configuration(void)
{
? RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);?? ?
? WWDG_SetPrescaler(WWDG_Prescaler_8);?? ? ? ? ? ? ? ? ?// ?WWDG clock counter = (PCLK1/4096)/8 = 244 Hz (~4 ms) ?
? WWDG_SetWindowValue(0x41);?? ??? ? ? ? ? ? ? ? ? ? // Set Window value to 0x41
? WWDG_Enable(0x50);?? ??? ? ? ? ? // Enable WWDG and set counter value to 0x7F, WWDG timeout = ~4 ms * 64 = 262 ms?
? WWDG_ClearFlag();?? ??? ??? ? ? ? ? // Clear EWI flag
? WWDG_EnableIT();?? ??? ??? ? ? ? ? // Enable EW interrupt
}
/*
********************************************************************************
** 函数名称 ： WWDG_IRQHandler(void)
** 函数功能 ： 窗口提前唤醒中断
** 输 ? ?入?? ?： 无
** 输 ? ?出?? ?： 无
** 返 ? ?回?? ?： 无
********************************************************************************
*/?
void WWDG_IRQHandler(void)
{
? /* Update WWDG counter */
? WWDG_SetCounter(0x50);
?? ?
? /* Clear EWI flag */
? WWDG_ClearFlag();?
}
?//************************************************
|