forked from matthew-t-watson/Picopter
-
Notifications
You must be signed in to change notification settings - Fork 0
/
AHRS.h
51 lines (41 loc) · 969 Bytes
/
AHRS.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
/*
* File: AHRS.h
* Author: matt
*
* Created on 29 October 2012, 22:38
*/
#ifndef AHRS_H
#define AHRS_H
#include <iostream>
#include "Timer.h"
#include "MPU6050.h"
#include "Kalman.h"
#include "ExtendedKalman.h"
#include "struct_rawData.h"
#include "struct_calibratedData.h"
#include "struct_euler.h"
#include "Quaternion.h"
const double g = 9.81816;
class AHRSClass
{
public:
AHRSClass();
AHRSClass(const AHRSClass& orig);
virtual ~AHRSClass();
void update();
s_calibratedData calibratedData;
s_rawData rawData_;
s_euler orientation;
QuaternionClass quaternion;
private:
void getSensors_();
void calibrateData_();
void temperatureCompensate_();
void fuse_();
void quaternionToYPR_(QuaternionClass* q, s_euler* orientation);
double magnitude_(double x, double y, double z);
// KalmanClass kalmanPhi_, kalmanPsi_;
ExtendedKalmanClass EKF;
};
extern AHRSClass AHRS;
#endif /* AHRS_H */