#include "bexus_mpu.h"

int32_t *rawTemp;
double accelScale = 16384;
double gyroScale = 131.072;
double *tempMpu;

struct vector_Uint *rawAccel, *rawGyro;

struct vector *hMeas;
struct vector *gSens;

void Mpu_Init() {

	if(HAL_I2C_IsDeviceReady(&hi2c2,MPU_ADDR,3,20)==HAL_OK){
		//HAL_GPIO_WritePin(GPIOB,GPIO_PIN_7,1);
	}
	//Sleepmode Beenden	
	ConfigMpu(0x6B, 0x01);
	
	//Konfigurieren	
//	ConfigMpu(0x1A, 0x05);//Low-Pass-Filter Config
//	ConfigMpu(0x1B, 0x00<<3);//Gyros Config
//	ConfigMpu(0x1C, 0x00<<3);//Achsel Config	
	
	ConfigMpu(0x1C, 0x00 << 3);//Achsel Config	2G
	ConfigMpu(0x1B, 0x00 << 3);//Gyros Config	250°/s
	ConfigMpu(0x1A, 0x04);//Low-Pass-Filter Config 20Hz LowPass
	
	
}

int Bexus_Mpu_Read() {

	rawAccel = &Dataset_Master[buff_sensor_data_index].accel_raw;
	rawGyro = &Dataset_Master[buff_sensor_data_index].gyro_raw;
	rawTemp = &Dataset_Master[buff_sensor_data_index].temperature_mpu_raw;
	
	
	hMeas = &Dataset_Master[buff_sensor_data_index].accel;
	gSens = &Dataset_Master[buff_sensor_data_index].gyro;
	tempMpu = &Dataset_Master[buff_sensor_data_index].temperature_mpu;
	
	//Auslesen

	//Beschleunigungssensor	

	rawAccel->x = recvIntFromMpu(MPU_ADDR_ACCEL);
	rawAccel->y = recvIntFromMpu(MPU_ADDR_ACCEL+2);
	rawAccel->z = recvIntFromMpu(MPU_ADDR_ACCEL+4);
	
	//Gyrosensor	
	rawGyro->x = recvIntFromMpu(MPU_ADDR_GYRO);
	rawGyro->y = recvIntFromMpu(MPU_ADDR_GYRO+2);
	rawGyro->z = recvIntFromMpu(MPU_ADDR_GYRO+4);

	//Temperatur
	*rawTemp=recvIntFromMpu(0x41);
		
		//*tempMpu = ((*rawTemp-0xFFFF0000)/340+36.53)/10;	
		*tempMpu = (double)((*rawTemp*1.)/340+36.53);	
		
	//berechnen	
	/*
	accel->x = RAD_TO_DEG * atan(rawAccel->x / sqrt(pow(rawAccel->y, 2) + pow(rawAccel->z, 2))); 
	accel->y = RAD_TO_DEG * atan(rawAccel->y / sqrt(pow(rawAccel->x, 2) + pow(rawAccel->z, 2)));
	accel->z = RAD_TO_DEG * atan(sqrt(pow(rawAccel->y,2) + pow(rawAccel->x,2)) / rawAccel->z);	
  */
#ifdef BOARD_HAS_ETHERNET //vertical
	
	hMeas->x = - rawAccel->x / accelScale + 0.0326;
	hMeas->y = - rawAccel->z / accelScale - 0.13;
	hMeas->z = - rawAccel->y / accelScale + 0.;
	
	gSens->x = - rawGyro->x/gyroScale - 3.0823; // °/s
	gSens->y = - rawGyro->z/gyroScale + 0.0824;
	gSens->z = - rawGyro->y/gyroScale + 0.336;
#else //horizontal

	hMeas->x = - rawAccel->x / accelScale + 0.03;
	hMeas->y = - rawAccel->y / accelScale - 0.0;
	hMeas->z = + rawAccel->z / accelScale + 0.126;
	
	gSens->x = - rawGyro->x/gyroScale - 3.014; // °/s
	gSens->y = - rawGyro->y/gyroScale + 0.633;
	gSens->z = + rawGyro->z/gyroScale - 0.496;

#endif
	return HAL_OK;
}


int sendByteToMpu(uint8_t* data, uint8_t len) {
	if(HAL_OK != HAL_I2C_Master_Transmit(&hi2c2,MPU_ADDR,data,len,25)) return HAL_ERROR;
	return HAL_OK;
}
void ConfigMpu(uint8_t reg, uint8_t data) {
	uint8_t butt_temp[2]={reg,data};
	sendByteToMpu(butt_temp, 2);
	//sendByteToMpu(&data, 1);
}
uint8_t recvByteFromMpu(uint8_t reg) {
	uint8_t buf[1];
	if (HAL_OK ==sendByteToMpu(&reg, 1))
	HAL_I2C_Master_Receive(&hi2c2,MPU_ADDR,buf,1,20);
	
	return buf[0];
}
int16_t recvIntFromMpu(uint8_t reg) {
	uint8_t buf[2];
	if (HAL_OK ==sendByteToMpu(&reg, 1))
	HAL_I2C_Master_Receive(&hi2c2,MPU_ADDR,&buf[0],1,25);
	
	reg++;
	if (HAL_OK ==sendByteToMpu(&reg, 1))
	HAL_I2C_Master_Receive(&hi2c2,MPU_ADDR,&buf[1],1,25);
	
	return (int16_t)(buf[1] | (buf[0] << 8));
}