#include "main.h"


//**************************************
//写入一个字节数据
//**************************************
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;
}

int MPU9250_init(void)
{
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x80)) return 1;                //复位所有寄存器，复位完后自动进入睡眠模式，所以要sleep位要清0
	nrf_delay_ms(5);
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_PWR_MGMT_1,0x00)) return 1;			//唤醒mpu9250
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_SMPLRT_DIV,0x07)) return 2;    		
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_CONFIG,0x00)) return 3;    			//低通滤波5hz
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_GYRO_CONFIG,0x18)) return 4;			//不自检，2000deg/s
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG,0x18)) return 5;			//(0x00 +-2g;)  ( 0x08 +-4g;)  (0x10 +-8g;)  (0x18 +-16g)
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG_2,0x04)) return 6;			//
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_INT_PIN_CFG,0x02)) return 7;
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_USER_CTRL,0x00)) return 8;				//使能I2C
	if(!MPU9250_Write_Byte(AK8963_I2C_ADDR,AK8963_CNTL1,0x0f)) return 9;
	return 0;
}

int 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 -1;//读取加速度,温度,陀螺仪传感器
	if(!MPU9250_Read_nBytes(AK8963_I2C_ADDR,AK8963_HXL,  bufM, 6)) 				return -2;//读取地磁传感器
	if(!MPU9250_Write_Byte(AK8963_I2C_ADDR,AK8963_CNTL1,0x11)) 					return -3;//每读一次数据，ak8963会自动进入powerdown模式,这里需要重新设定为单测量模式
	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]);
	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 0;
}