added code handling IMU and started writing stabilization code

Core/Inc/MPU6000.h

@@ -0,0 +1,120 @@
+/*
+ * 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_ */