345 lines
10 KiB
C
345 lines
10 KiB
C
#include "main.h"
|
|
#include "mmc5983.h"
|
|
#include "UartDebug.h"
|
|
#include "i2c.h"
|
|
#include "Algorithm.h"
|
|
//test
|
|
|
|
static int32_t max_in[XYZ];
|
|
static int32_t min_in[XYZ];
|
|
static bool InitFlag;
|
|
static bool MMC5983IntFlag;
|
|
static MMC5983Var_t MMC5983Var;
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: MMC5983_Write_Reg
|
|
* 功能描述: MMC5983写寄存器
|
|
* 参 数: reg 寄存器地址
|
|
val 寄存器数据
|
|
* 返 回 值: 成功返回true，失败返回false
|
|
*****************************************************************************************/
|
|
static bool MMC5983_Write_Reg(uint8_t reg, uint8_t val)
|
|
{
|
|
return I2C_MasterWriteData(M0P_I2C0, MMC5983_ADDRESS, reg, &val, 1);
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: MMC5983_Read_Reg
|
|
* 功能描述: MMC5983读寄存器
|
|
* 参 数: reg 寄存器地址
|
|
* 返 回 值: 返回寄存器值
|
|
*****************************************************************************************/
|
|
static uint8_t MMC5983_Read_Reg(uint8_t reg)
|
|
{
|
|
uint8_t res;
|
|
if(I2C_MasterReadData(M0P_I2C0, MMC5983_ADDRESS, reg, &res, 1) == false)
|
|
res = 0;
|
|
return res;
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: MMC5983_Read_Buffer
|
|
* 功能描述: MMC5983读数据
|
|
* 参 数: reg 寄存器地址
|
|
buffer 读取数据输出缓存入口
|
|
len 读取长度
|
|
* 返 回 值: 成功返回true，失败返回false
|
|
*****************************************************************************************/
|
|
static bool MMC5983_Read_Buffer(uint8_t reg, void *buffer, uint8_t len)
|
|
{
|
|
return I2C_MasterReadData(M0P_I2C0, MMC5983_ADDRESS, reg, buffer, len);
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: MMC5983StartCov
|
|
* 功能描述: MMC5983开启连续转换
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
static void MMC5983StartCov(void)
|
|
{
|
|
MMC5983RegCtrl2 Ctrl2Reg; //开启/关闭连续转换
|
|
Ctrl2Reg.Byte = 0x00;
|
|
Ctrl2Reg.Bit.Cm_freq = 0x04;
|
|
Ctrl2Reg.Bit.Cmm_en = 1; //连续设1，单次设0
|
|
Ctrl2Reg.Bit.Prd_set = 3;
|
|
Ctrl2Reg.Bit.En_prd_set = 1; //连续设1，单次设0
|
|
MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: MMC5983_Init
|
|
* 功能描述: MMC5983初始化
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
static void MMC5983_Init(void)
|
|
{
|
|
uint8_t id;
|
|
|
|
id = MMC5983_Read_Reg(MMC5983_ID);
|
|
//DBG_LOG("MMC5983 id=0x%x\r\n",id);
|
|
MMC5983Var.MMC5983Delay1mSCnt = 100;
|
|
while(id != 0x30){
|
|
if(MMC5983Var.MMC5983Delay1mSCnt <= 0) {
|
|
DBG_LOG("LEFT MMC5983 INIT Err...\r\n");
|
|
InitFlag = false;
|
|
return;
|
|
}
|
|
}
|
|
|
|
MMC5983RegCtrl1 Ctrl1Reg;
|
|
Ctrl1Reg.Byte = 0x00;
|
|
Ctrl1Reg.Bit.SW_RST = 1;
|
|
MMC5983_Write_Reg(MMC5983_CTRL_1, Ctrl1Reg.Byte);
|
|
MMC5983Delay(15);
|
|
|
|
MMC5983RegCtrl2 Ctrl2Reg; //开启/关闭连续转换
|
|
Ctrl2Reg.Byte = 0x00;
|
|
Ctrl2Reg.Bit.Cm_freq = 0x04;
|
|
Ctrl2Reg.Bit.Cmm_en = 0; //连续设1，单次设0
|
|
Ctrl2Reg.Bit.Prd_set = 3;
|
|
Ctrl2Reg.Bit.En_prd_set = 0; //连续设1，单次设0
|
|
MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);
|
|
|
|
MMC5983RegCtrl3 Ctrl3Reg;
|
|
Ctrl3Reg.Byte = 0x00;
|
|
Ctrl3Reg.Bit.St_enm = 1;
|
|
Ctrl3Reg.Bit.St_enp = 1;
|
|
MMC5983_Write_Reg(MMC5983_CTRL_3, Ctrl3Reg.Byte);
|
|
|
|
MMC5983RegCtrl0 Ctrl0Reg;
|
|
Ctrl0Reg.Byte = 0x00;
|
|
Ctrl0Reg.Bit.TM_M = 1;
|
|
Ctrl0Reg.Bit.Auto_SR_en = 1;
|
|
Ctrl0Reg.Bit.NT_meas_done_en = 1;
|
|
MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);
|
|
|
|
if(InitFlag == false){
|
|
DBG_LOG("MMC5983 INIT OK...\r\n");
|
|
InitFlag = true;
|
|
}
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: MMC5983_Read
|
|
* 功能描述: MMC5983读取原始数据
|
|
* 参 数: MagAdcX X轴数据
|
|
MagAdcY Y轴数据
|
|
MagAdcZ Z轴数据
|
|
* 返 回 值: 成功返回true，失败返回false
|
|
*****************************************************************************************/
|
|
static bool MMC5983_Read(int32_t *MagAdcX, int32_t *MagAdcY, int32_t *MagAdcZ)
|
|
{
|
|
static uint8_t buff[7];
|
|
|
|
if (MMC5983_Read_Buffer(MMC5983_X_OUT_H, buff, 7) == false)
|
|
return false;
|
|
|
|
*MagAdcX = (buff[0] << 10) | (buff[1] << 2) | ((buff[6] & 0xC0) >> 6);
|
|
*MagAdcY = (buff[2] << 10) | (buff[3] << 2) | ((buff[6] & 0x30) >> 4);
|
|
*MagAdcZ = (buff[4] << 10) | (buff[5] << 2) | ((buff[6] & 0x0C) >> 2);// Turn the 18 bits into unsigned 32-bit value
|
|
return true;
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: MMC5983_Get_Mag
|
|
* 功能描述: 外部获取MMC5983磁场强度数据
|
|
* 参 数: mag 读取数据输出缓存入口
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
void MMC5983_Get_Mag(float *mag)
|
|
{
|
|
if(mag == NULL)
|
|
return;
|
|
|
|
float ADVal[XYZ];
|
|
|
|
ADVal[X] = MMC5983Var.Mag_Original[X] - 131072.0f;
|
|
ADVal[Y] = MMC5983Var.Mag_Original[Y] - 131072.0f;
|
|
ADVal[Z] = MMC5983Var.Mag_Original[Z] - 131072.0f;
|
|
|
|
mag[0] = (float) (ADVal[X] * MMC5983_MAG_SCALE_18BIT);
|
|
mag[1] = (float) (ADVal[Y] * MMC5983_MAG_SCALE_18BIT);
|
|
mag[2] = (float) (ADVal[Z] * MMC5983_MAG_SCALE_18BIT);
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: MMC5983LoopHandler
|
|
* 功能描述: MMC5983循环处理函数
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
extern void MmcDataProcessCallBack(float *Mag);
|
|
|
|
void MMC5983LoopHandler(void)
|
|
{
|
|
static int32_t mag_array[XYZ][10];
|
|
static uint8_t magcnt;
|
|
static uint8_t TimeOutMagcnt;
|
|
|
|
switch(MMC5983Var.Status) {
|
|
case MMC5983_STATUS_IDLE:{
|
|
break;}
|
|
|
|
case MMC5983_STATUS_START:{
|
|
for(int i = 0; i < 10; i++) {
|
|
mag_array[X][i] = 0;
|
|
mag_array[Y][i] = 0;
|
|
mag_array[Z][i] = 0;
|
|
}
|
|
magcnt = 0;
|
|
TimeOutMagcnt = 0;
|
|
memset(&MMC5983Var, NULL, sizeof(MMC5983Var));
|
|
MMC5983Var.Status = MMC5983_STATUS_DELAY;
|
|
break;}
|
|
|
|
case MMC5983_STATUS_DELAY:{
|
|
MMC5983StartRead();
|
|
MMC5983Var.MMC5983Delay1mSCnt = 50;
|
|
MMC5983Var.Status = MMC5983_STATUS_WAIT_DELAY;
|
|
break;}
|
|
|
|
case MMC5983_STATUS_WAIT_DELAY:{
|
|
if(MMC5983Var.MMC5983Delay1mSCnt <= 0){
|
|
MMC5983Var.Status = MMC5983_STATUS_READ;
|
|
}
|
|
break;}
|
|
|
|
case MMC5983_STATUS_READ:{
|
|
MMC5983Var.MMC5983Delay1mSCnt = 50;
|
|
MMC5983Var.Status = MMC5983_STATUS_WAIT_READ;
|
|
break;}
|
|
|
|
case MMC5983_STATUS_WAIT_READ:{
|
|
if(MMC5983Var.MMC5983Delay1mSCnt <= 0){
|
|
MMC5983Var.Status = MMC5983_STATUS_DELAY;
|
|
TimeOutMagcnt++;
|
|
}
|
|
if(MMC5983IntFlag == true){
|
|
MMC5983IntFlag = false;
|
|
MMC5983_Read(&mag_array[X][magcnt], &mag_array[Y][magcnt], &mag_array[Z][magcnt]);
|
|
magcnt++;
|
|
MMC5983Var.Status = MMC5983_STATUS_DELAY;
|
|
}
|
|
if(TimeOutMagcnt >= 10){
|
|
TimeOutMagcnt = 0;
|
|
if(magcnt > 0){
|
|
MMC5983Var.Mag_Original[X] = AverageFilter_32t(mag_array[X], magcnt);
|
|
MMC5983Var.Mag_Original[Y] = AverageFilter_32t(mag_array[Y], magcnt);
|
|
MMC5983Var.Mag_Original[Z] = AverageFilter_32t(mag_array[Z], magcnt);
|
|
magcnt = 0;
|
|
MMC5983_Get_Mag(MMC5983Var.Mag);
|
|
MmcDataProcessCallBack(MMC5983Var.Mag);
|
|
MMC5983Var.Status = MMC5983_STATUS_STOP;
|
|
}
|
|
else{
|
|
MMC5983Var.Status = MMC5983_STATUS_STOP;
|
|
// DBG_LOG("LEFT MMC5983 TimeOut...\r\n");
|
|
}
|
|
}
|
|
if(magcnt >= 10){
|
|
MMC5983Var.Mag_Original[X] = AverageFilter_32t(mag_array[X], magcnt);
|
|
MMC5983Var.Mag_Original[Y] = AverageFilter_32t(mag_array[Y], magcnt);
|
|
MMC5983Var.Mag_Original[Z] = AverageFilter_32t(mag_array[Z], magcnt);
|
|
magcnt = 0;
|
|
MMC5983_Get_Mag(MMC5983Var.Mag);
|
|
MmcDataProcessCallBack(MMC5983Var.Mag);
|
|
MMC5983Var.Status = MMC5983_STATUS_STOP;
|
|
}
|
|
break;}
|
|
|
|
case MMC5983_STATUS_STOP:{
|
|
MMC5983Stop();
|
|
MMC5983Var.Status = MMC5983_STATUS_IDLE;
|
|
//MMC5983Var.Status = MMC5983_STATUS_START;//持续工作
|
|
break;}
|
|
}
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: LeftMMC59831mSRoutine
|
|
* 功能描述: MMC59831ms时基循环处理函数
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
void MMC59831mSRoutine(void)
|
|
{
|
|
if(MMC5983Var.MMC5983Delay1mSCnt > 0)
|
|
MMC5983Var.MMC5983Delay1mSCnt--;
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: LeftMMC5983Init
|
|
* 功能描述: MMC5983初始化
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
void MMC5983Init(void)
|
|
{
|
|
if(!InitFlag){
|
|
MMC5983_Init();
|
|
return;
|
|
}
|
|
|
|
MMC5983Var.Status = MMC5983_STATUS_START;
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: LeftMMC5983Start
|
|
* 功能描述: MMC5983启动
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
void MMC5983Start(void)
|
|
{
|
|
if(!InitFlag){
|
|
MMC5983_Init();
|
|
return;
|
|
}
|
|
|
|
MMC5983Var.Status = MMC5983_STATUS_START;
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: LeftMMC5983StartRead
|
|
* 功能描述: MMC5983启动读取
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
void MMC5983StartRead(void)
|
|
{
|
|
MMC5983_Init();//读取时重新初始化
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: MMC5983Stop
|
|
* 功能描述: MMC5983停止
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
void MMC5983Stop(void)
|
|
{
|
|
MMC5983RegCtrl0 Ctrl0Reg;
|
|
Ctrl0Reg.Byte = 0x00;
|
|
MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);
|
|
|
|
MMC5983RegStatus StatusReg;
|
|
StatusReg.Byte = MMC5983_Read_Reg(MMC5983_STATUS);
|
|
if(StatusReg.Bit.Meas_M_Done == 1){
|
|
MMC5983_Write_Reg(MMC5983_STATUS, StatusReg.Byte);
|
|
}
|
|
}
|
|
|
|
/*****************************************************************************************
|
|
* 函数名称: LeftMMC5983IntCallBack
|
|
* 功能描述: MMC5983中断回调
|
|
* 参 数: 无
|
|
* 返 回 值: 无
|
|
*****************************************************************************************/
|
|
void MMC5983IntCallBack(void)
|
|
{
|
|
MMC5983IntFlag = true;
|
|
}
|
|
|