/*
 * MPU6000.h
 *
 *  Created on: Nov 24, 2021
 *      Author: angoosh
 */

#ifndef INC_MPU6000_H_
#define INC_MPU6000_H_

#include "stm32g0xx_hal.h"

#define MPU6000_ADDRESS		0x68//0x68

#define ACCEL_RANGE			0		//2048 LSB/g
#define GYRO_RANGE			0		//32.8 LSB/deg/s
#define PERFORM_SELFTEST	0

//register description https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
typedef enum {
	GYRO_CONFIG = 0x1B,// register 27
	ACCEL_CONFIG = 0x1C,// register 28
	FIFO_EN = 0x23,
	ACCEL_XOUT_H = 0x3B,
	ACCEL_XOUT_L = 0x3C,
	ACCEL_YOUT_H = 0x3D,
	ACCEL_YOUT_L = 0x3E,
	ACCEL_ZOUT_H = 0x3F,
	ACCEL_ZOUT_L = 0x40,//sampl rate = reg 25 ; scale in reg 28
	TEMP_OUT_H = 0x41,
	TEMP_OUT_L = 0x42,
	GYRO_XOUT_H = 0x43,
	GYRO_XOUT_L = 0x44,
	GYRO_YOUT_H = 0x45,
	GYRO_YOUT_L = 0x46,
	GYRO_ZOUT_H = 0x47,
	GYRO_ZOUT_L = 0x48,// register 27 full scale setup
	SENSOR_RESET = 0x68,
	PWR_MGMT_1 = 0x6B,
	PWR_MGMT_2 = 0x6C,
	SMPLRT_DIV = 0x19,//register 25
	WHO_AM_I = 0x75
} MPU6000_REG;

typedef enum {
	ACCEL_16384_LSB_g = 0,
	ACCEL_8192_LSB_g = 1,
	ACCEL_4096_LSB_g = 2,
	ACCEL_2048_LSB_g = 3,
	GYRO_131_LSB_deg_s = 0,
	GYRO_65_5_LSB_deg_s = 1,
	GYRO_32_8_LSB_deg_s = 2,
	GYRO_16_4_LSB_deg_s = 3,
} MPU6000_SCALE;

typedef struct{
	float g_x;
	float g_y;
	float g_z;
	float a_x;
	float a_y;
	float a_z;
	int16_t cal_g_x;
	int16_t cal_g_y;
	int16_t cal_g_z;
	int16_t cal_a_x;
	int16_t cal_a_y;
	int16_t cal_a_z;
	float pitch;
	float roll;
	float yaw;
	uint8_t gyro_data[6];
	uint8_t accel_data[6];
	uint8_t all_data[14];
	float gyro_LSB;
	float accel_LSB;
	uint32_t pedo_steps;
	float float_pedo_steps;
	float pedo_threshold;
	float pedo_work_vector[100];
	float pedo_vector[100];
	float pedo_a_x[100];
	float pedo_a_y[100];
	float pedo_a_z[100];
	float pedo_avg_a_x;
	float pedo_avg_a_y;
	float pedo_avg_a_z;
	uint8_t pedo_index;
	uint8_t pedo_flag;
	float pedo_vector_max;
	float pedo_vector_avg;
	uint32_t pedo_vector_samples;
	uint32_t pedo_last_step_timestamp;
	uint8_t rx_expected_flag;
	I2C_HandleTypeDef *peripheral;
	uint32_t timeout;
	uint8_t dataReady;
} MPU6000_Typedef;

extern MPU6000_Typedef IMU;



void MPU6000_readGyro();
void MPU6000_readAccel();
int MPU6000_Init(I2C_HandleTypeDef *hi2c, uint32_t timeout);
uint8_t MPU6000_readRegister(uint8_t reg);
void MPU6000_Calibrate();
void MPU6000_Gyro();
void MPU6000_Raw_Data_Convert();
void MPU6000_readAll();

void MPU6000_Pedometer_Init();
void MPU6000_Pedometer();

void MPU6000_I2C_CallbackFunc(I2C_HandleTypeDef *hi2c);

void IMU_Check_State();

#endif /* INC_MPU6000_H_ */