#include <stdbool.h>
#include <stdint.h>
#include "nrf_delay.h"
#include "twi_master.h"
#include "mpu9250.h"


MPU_t mMPU9250;

//**************************************
//写入一个字节数据
//**************************************
bool MPU9250_Write_Byte(uint8_t Device_Address,uint8_t REG_Address,uint8_t REG_data)
{
	uint8_t buf[2];

    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;
}

uint8_t MPU9250_init(void)
{
	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,0x01)) return 2;    			//低通滤波5hz
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_GYRO_CONFIG,0x18)) return 3;			//不自检，2000deg/s
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG,0x10)) return 4;			//(0x00 +-2g;)  ( 0x08 +-4g;)  (0x10 +-8g;)  (0x18 +-16g)
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_ACCEL_CONFIG_2,0x04)) return 4;			//
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_INT_PIN_CFG,0x02)) return 5;
	if(!MPU9250_Write_Byte(MPU9250_I2C_ADDR,MPU9250_USER_CTRL,0x00)) return 6;				//使能I2C
	
	if(!MPU9250_Write_Byte(AK8963_I2C_ADDR,AK8963_CNTL1,0x0f)) return 7;
	nrf_delay_ms(50);
	MPU9250_Write_Byte(AK8963_I2C_ADDR,AK8963_CNTL1,0x11);
	return 0;
}   

void MPU9250_ReadData(void)
{
	uint8_t buf[14],bufM[6];    			
	MPU9250_Read_nBytes(MPU9250_I2C_ADDR,MPU9250_ACCEL_XOUT_H,  buf, 14);//读取加速度,温度,陀螺仪传感器
	MPU9250_Read_nBytes(AK8963_I2C_ADDR,AK8963_HXL,  bufM, 6);//读取地磁传感器
	MPU9250_Write_Byte(AK8963_I2C_ADDR,AK8963_CNTL1,0x11);//每读一次数据，ak8963会自动进入powerdown模式,这里需要重新设定为单测量模式
	mMPU9250.acc_x  = ((int16_t)buf[0]<<8)|((int16_t)buf[1]);
	mMPU9250.acc_y  = ((int16_t)buf[2]<<8)|((int16_t)buf[3]);
	mMPU9250.acc_z  = ((int16_t)buf[4]<<8)|((int16_t)buf[5]);
	mMPU9250.temp   = ((int16_t)buf[6]<<8)|((int16_t)buf[7]);
	mMPU9250.gyro_x = ((int16_t)buf[8]<<8)|((int16_t)buf[9]);
	mMPU9250.gyro_y = ((int16_t)buf[10]<<8)|((int16_t)buf[11]);
	mMPU9250.gyro_z = ((int16_t)buf[12]<<8)|((int16_t)buf[13]);
	mMPU9250.mag_x  = ((int16_t)bufM[1]<<8)|((int16_t)bufM[0]);
	mMPU9250.mag_y  = ((int16_t)bufM[3]<<8)|((int16_t)bufM[2]);
	mMPU9250.mag_z  = ((int16_t)bufM[5]<<8)|((int16_t)bufM[4]);
}