#include "drv_mpu9250.h"
#include "nrf_delay.h"
#include "twi_master.h"
#include "drv_iic_01.h"
#include "drv_mpu9250.h"
#include "drv_iic_01.h"

bool MPU9250_Write_Byte(uint8_t Device_Address,uint8_t REG_Address,uint8_t REG_data)
{
	return  IIC_01_WriteBytes(Device_Address,REG_Address,&REG_data,1);
}

bool MPU9250_Read_nBytes(uint8_t Device_Address,uint8_t REG_Address,uint8_t *readDataBuf,uint8_t readDataLen)
{
	return IIC_01_ReadBytes(Device_Address,REG_Address,readDataBuf,readDataLen);
}
bool MPU9250_register_write_len(uint8_t Device_Address,uint8_t register_address, uint8_t len,uint8_t *buf)
{
	return IIC_01_WriteBytes(Device_Address,register_address,buf,len);
}
bool MPU9250_register_read_len(uint8_t Device_Address,uint8_t register_address, uint8_t number_of_bytes,uint8_t * destination )
{
	return IIC_01_ReadBytes(Device_Address,register_address,destination,number_of_bytes);
}

//**************************************
//写入一个字节数据
////**************************************
//bool MPU9250_Write_Byte(uint8_t Device_Address,uint8_t REG_Address,uint8_t REG_data)
//{
//	  uint8_t buf[2];
//	  SEGGER_RTT_printf(0,"MPU9250_Write_Byte\n");
//    buf[0] = REG_Address;
//    buf[1] = REG_data;
//    return twi_master_transfer(Device_Address, buf, 2, TWI_ISSUE_STOP);
//}

//**************************************
//读取N个字节数据
//**************************************
//bool MPU9250_Read_nBytes(uint8_t Device_Address,uint8_t REG_Address,uint8_t *readDataBuf,uint8_t readDataLen)/*
//{
//	bool ret;
//    ret  = twi_master_transfer(Device_Address, &REG_Address, 1, TWI_DONT_ISSUE_STOP);
//    ret &= twi_master_transfer(Device_Address|TWI_READ_BIT, readDataBuf, readDataLen, TWI_ISSUE_STOP);
//    return ret;
//}


//bool MPU9250_register_write_len(uint8_t Device_Address,uint8_t register_address, uint8_t len,uint8_t *buf)
//{
//    uint8_t w2_data[50],i;

//    w2_data[0] = register_address;
//	  for(i=0;i<len;i++)w2_data[i+1] = *(buf+i);

//	  
//	  if(twi_master_transfer(Device_Address, w2_data, len+1, TWI_ISSUE_STOP) == true)return 0;
//	  else return true;
//}

//bool MPU9250_register_read_len(uint8_t Device_Address,uint8_t register_address, uint8_t number_of_bytes,uint8_t * destination )
//{
//    bool transfer_succeeded;
//    transfer_succeeded  = twi_master_transfer(Device_Address, &register_address, 1, TWI_DONT_ISSUE_STOP);
//    transfer_succeeded &= twi_master_transfer(Device_Address|TWI_READ_BIT, destination, number_of_bytes, TWI_ISSUE_STOP);
//    
//	  if(transfer_succeeded == true)return 0;
//	  else return true;
//}

bool MPU9250_Reset(void)
{
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x80)) return false;                //复位所有寄存器，复位完后自动进入睡眠模式，所以要sleep位要清0
	SEGGER_RTT_printf(0,"MPU9250_Reset OK\n");
	return true;
}

bool MPU9250_sleep(void)
{
	  if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x00)) return false;                  //唤醒mpu9250
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_2,0x3f)) return false;                  //关闭陀螺仪和三轴
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x60)) return false;                  //enable cycle mode (accel low power mode)
		SEGGER_RTT_printf(0,"MPU9250_Acc_Init OK\n");
		return true;
}

bool MPU9250_MageInit(void)
{
	if(!MPU9250_Write_Byte(AK8963_I2C_ADDR,AK8963_CNTL1,0x00)) return false;    //power-down mode
	SEGGER_RTT_printf(0,"MPU9250_MageInit OK\n");
	return true;
}

bool MPU9250_Acc_Init(void)
{
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x00)) return false;                  //唤醒mpu9250
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_2,0x07)) return false;                  //关闭陀螺仪三轴
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG,0x18)) return false;                //(0x00 +-2g;)  ( 0x08 +-4g;)  (0x10 +-8g;)  (0x18 +-16g)
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG_2,0x09)) return false;        //set accel lpf setting to 184 hz bandwidth
//        if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_LP_ACCEL_ODR,0x02)) return false;          //set frequency of wake-up 低功耗加速度计输出速率0.98hz
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_LP_ACCEL_ODR,0x07)) return false;          //set frequency of wake-up 低功耗加速度计输出速率31.25hz
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x20)) return false;                        //enable cycle mode (accel low power mode)
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_INT_PIN_CFG,0x02)) return false;                        //bypass enable
		if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_USER_CTRL,0x00)) return false;                               //使能I2C
		SEGGER_RTT_printf(0,"MPU9250_Acc_Init OK\n");
		return true;
}

bool MPU9250_Read_Acc(int16_t* acc)
{
	uint8_t buf[6];
//	SEGGER_RTT_printf(0,"MPU9250_Read_nBytes=%d\n",MPU9250_Read_nBytes(MPU9250_I2C_ADDR,MPU9250_ACCEL_XOUT_H,  buf, 6));
	if(!MPU9250_Read_nBytes(MPU9250_I2C_ADDR,MPU9250_ACCEL_XOUT_H,  buf, 6)) 	return false;//读取加速度,温度,陀螺仪传感器
	acc[0]  = ((int16_t)buf[0]<<8)|((int16_t)buf[1]);
	acc[1]  = ((int16_t)buf[2]<<8)|((int16_t)buf[3]);
	acc[2]  = ((int16_t)buf[4]<<8)|((int16_t)buf[5]);
	return true;
}

//int MPU9250_LowPowerAccelModeInit(void)
//{
//        if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x00)) return 2;                  //唤醒mpu9250
//        if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_2,0x07)) return 3;                  //关闭陀螺仪三轴
//        if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG,0x18)) return 4;                //(0x00 +-2g;)  ( 0x08 +-4g;)  (0x10 +-8g;)  (0x18 +-16g)
//        if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG_2,0x09)) return 5;        //set accel lpf setting to 184 hz bandwidth
//        if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_LP_ACCEL_ODR,0x02)) return 6;          //set frequency of wake-up 低功耗加速度计输出速率0.98hz
//        if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x20)) return 7;                        //enable cycle mode (accel low power mode)
//        if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_INT_PIN_CFG,0x02)) return 8;                        //bypass enable
//        if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_USER_CTRL,0x00)) return 9;                                //使能I2C
//        if(!MPU9250_Write_Byte(AK8963_I2C_ADDR,AK8963_CNTL1,0x00)) return 10;                                                        //power-down mode
//        
//        return 0;
//}

bool MPU9250_Init(void)
{
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x00)) return false;			//唤醒mpu9250
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_SMPLRT_DIV,0x07)) return false;    		
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_CONFIG,0x00)) return false;    				//低通滤波5hz
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_GYRO_CONFIG,MPU9250_GYR_SEL)) return false;	//不自检，2000deg/s
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG,MPU9250_ACC_SEL)) return false;	//(0x00 +-2g;)  ( 0x08 +-4g;)  (0x10 +-8g;)  (0x18 +-16g)
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG_2,0x04)) return false;			//
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_INT_PIN_CFG,0x02)) return false;
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_USER_CTRL,0x00)) return false;				//使能I2C
	if(!MPU9250_Write_Byte(AK8963_I2C_ADDR,AK8963_CNTL1,0x0f)) return false;
	SEGGER_RTT_printf(0,"MPU9250_Init OK\n");
	return true;
}

bool MPU9250_Read(int16_t* acc,int16_t* gyr,int16_t* mag,int16_t* temperature)
{
	uint8_t buf[14],bufM[6];    			
	if(!MPU9250_Read_nBytes(MPU9250_I2C_ADDR,MPU9250_ACCEL_XOUT_H,  buf, 14)) 	return false;//读取加速度,温度,陀螺仪传感器
	if(!MPU9250_Read_nBytes(AK8963_I2C_ADDR,AK8963_HXL,  bufM, 6)) 				return false;//读取地磁传感器
	acc[0]  = ((int16_t)buf[0]<<8)|((int16_t)buf[1]);
	acc[1]  = ((int16_t)buf[2]<<8)|((int16_t)buf[3]);
	acc[2]  = ((int16_t)buf[4]<<8)|((int16_t)buf[5]);
	temperature[0]   = ((int16_t)buf[6]<<8)|((int16_t)buf[7]);
	gyr[0] = ((int16_t)buf[8]<<8)|((int16_t)buf[9]);
	gyr[1] = ((int16_t)buf[10]<<8)|((int16_t)buf[11]);
	gyr[2] = ((int16_t)buf[12]<<8)|((int16_t)buf[13]);
	if(!MPU9250_Write_Byte(AK8963_I2C_ADDR,AK8963_CNTL1,0x11)) 					return false;//每读一次数据，ak8963会自动进入powerdown模式,这里需要重新设定为单测量模式
	
	mag[0]  = ((int16_t)bufM[1]<<8)|((int16_t)bufM[0]);
	mag[1]  = ((int16_t)bufM[3]<<8)|((int16_t)bufM[2]);
	mag[2]  = ((int16_t)bufM[5]<<8)|((int16_t)bufM[4]);
	return true;
}