/*
 * mpu6050.h
 *
 *  Created on: Nov 14, 2024
 *      Author: Pima
 */
#include "FreeRTOS.h"
#include "task.h"
#include "madgwick.h"


// struct to hold the settings for a MPU 6050
// typedef struct so you don't have to use struct to declare it
typedef struct {
	uint8_t address;				// I2C sensor address
	I2C_HandleTypeDef* hi2c;		// I2C handle
	float accel_bias[3];			// accelerometer calibration bias
	float gyro_bias[3];				// gyroscope calibration bias
	uint8_t accel_range;			// accelerometer range
	uint8_t gyro_range;				// gyroscope range
	float accel_factor;				// accelerometer conversion factor
	float gyro_factor;				// gyro conversion factor
	uint8_t calibrate;				// calibration flag
} MPU6050_HandleTypeDef;


//struct to hold the processed data for each sensor
typedef struct {
	float Sensor1Data[6];
	float Sensor2Data[6];
} ProcessedData_HandleTypeDef;

// dataframe for sending quaternions over uart
typedef struct __attribute__((packed)) {
	uint8_t StartByte;
	uint8_t SensorAddress;
	float qw;
	float qx;
	float qy;
	float qz;
	uint8_t EndByte;
} QuaternionData;

// enum to hold the packet dataframe data
typedef enum {
	PACKET_START_BYTE = 0xDE,
	PACKET_END_BYTE = 0xAD
} Packet_Bytes;

// hold the addresses of the MPU6050 registers
typedef enum {
	MPU6050_CONF_REG = 0x1A,
	MPU6050_GYR_CONF_REG = 0x1B,
	MPU6050_ACC_CONF_REG = 0x1C,
	MPU6050_FIFO_REG = 0x23,
	MPU6050_PWR_MNG_REG = 0x6B,
	MPU6050_SMP_RATE_REG = 0x19,
	MPU6050_GYR_MEAS_REG = 0x43,
	MPU6050_ACC_MEAS_REG = 0x3B
} MPU6050_Registers;


extern UART_HandleTypeDef *huart_mpu;
extern MPU6050_HandleTypeDef *hmpu;
extern TaskHandle_t xSensorReadingTask;
extern volatile int16_t raw_data[6]; 		//[ax, ay, az, gx, gy, gz]



float* process_binaryData (MPU6050_HandleTypeDef *mpu, uint8_t binarySensorData[], float processedData[]);
void read_dma (MPU6050_HandleTypeDef *mpu, uint8_t binaryData[]);

void sendQuat (UART_HandleTypeDef *uart, MPU6050_HandleTypeDef *mpu, MadgwickFilter *quat);

HAL_StatusTypeDef check_mpu6050_ready(MPU6050_HandleTypeDef *mpu);
void Init_MPU6050 (MPU6050_HandleTypeDef*, UART_HandleTypeDef*);
void setRange_MPU (MPU6050_HandleTypeDef *, UART_HandleTypeDef *);

void calibrate_Sensor(MPU6050_HandleTypeDef *mpu, UART_HandleTypeDef *uart);


void MPU_Diagnostics(MPU6050_HandleTypeDef *, UART_HandleTypeDef *);
void I2C_Scanner(I2C_HandleTypeDef *hi2c, UART_HandleTypeDef *uart);
void checkRegisters(MPU6050_HandleTypeDef *, UART_HandleTypeDef *);





//void read_accelerometer (I2C_HandleTypeDef*, UART_HandleTypeDef*);//, UART_HandleTypeDef);
//void read_gyro(I2C_HandleTypeDef*, UART_HandleTypeDef*);
//void sendData();
