49 lines
1.6 KiB
C++
49 lines
1.6 KiB
C++
|
/*
|
||
|
|
* 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_ */
|