/*
 * imu.hpp
 *
 *  Created on: 09.01.2021
 *      Author: frank
 */

#ifndef IMU_HPP_
#define IMU_HPP_

#include "mpu6050.hpp"

#define RADIAN_CONST 57.296; // = 1 / (PI / 180)
#define CONST_TO_RADIANS  (PI / 180)

#define DRIFT_CORRECTION_RATE_GYRO 0.9996;
#define DRIFT_CORRECTION_RATE_ACCEL 0.0004;

struct inertia {
	double roll_acceleration;
	double pitch_acceleration;
	double roll_gyroscope;
	double pitch_gyroscope;
	double vector_length;
	double roll_vector_length;
	double pitch_vector_length;
	double yaw_vector_length;
};

void calculate_inertia(inertia &inertia, const measurement measurement);
void calculate_inertia_with_offset_substraction(inertia &inertia,
		const measurement measurement);
void correct_inertia_drift(inertia &inertia);
void initialize_inertia(inertia &inertia, const measurement measurement);
double pitch_by_acceleration(const inertia inertia, const measurement measurement);
double roll_by_acceleration(const inertia inertia, const measurement measurement);
void substract_acceleration_offset(inertia &inertia);
double yaw_by_acceleration(const inertia inertia, const measurement measurement);
double vector_length_2d(const long x, const long y);
double vector_length_3d(measurement const measurement);
double roll_vector_length(const measurement mpu_measurement);
double pitch_vector_length(const measurement mpu_measurement);
double yaw_vector_length(const measurement mpu_measurement);
double acceleration_with_precission(const long value);
double gyroscope_with_precission(const long value);
inertia get_inertia_offset();

#endif /* IMU_HPP_ */