InertialNavigationSystem/main.cpp at master · JTiefnig/InertialNavigationSystem · GitHub

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
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
#include "mbed.h"
#include "Adafruit_SSD1306.h"
#include <vector>

using namespace std;

#include "Counter.h"
#include "MPU6050/MPU6050.h"
#include "Quaternion.h"


#include "SerialProtocol.h"
#include "Quaternion.h"

#include <math.h>

#include "myInit.h"

#define PI 3.14159265358979f

#define RADtoDEG 180/PI
#define DEGtoRAD PI/180

Ticker updater;

DigitalIn setToZero(PA_12);//D2

double dt =0;

void updateFunc()
{
    if(myled.read())
    myled = 0;
    else
    myled = 1;
}


int main()
{
    // Initalize Serial Port
    SerialProtocol serialPc(USBTX, USBRX);

    // Oled Display // shows splash screen during calibration...
    I2CPreInit gI2C(PB_7,PB_6);
    Adafruit_SSD1306_I2c gOled2(gI2C,PB_5, 0x78, 64, 128);

    // Initialize MPU
    mpu.init();
    // Calibrate Sensor - Dont move senosr on startup!!
    mpu.Calibrate();

    gOled2.clearDisplay();
    gOled2.setTextSize(1);

    int counter =0;

    updater.attach(&updateFunc, 0.02);

    t.start();

    while(1) {


        // Possibility to Zero the Roation
        if(setToZero)
            q = Quaternion();


        //Read in Gyro Data via library function
        DataVector read = mpu.GetGyroData();

        // Normalize values accroding to datasheet
        // get the yaw rate in the local sensor frame
        double dalpha = read.X *250.0/ 32768.0;
        double dbeta = read.Y  *250.0/ 32768.0;
        double dgamma = read.Z *250.0/ 32768.0;

        // to get the timeincremet right
        dt = t.read();
        t.reset(); // timer t zurücksetzen


        // --------Approach A
        // create a Quaternion that represenst
        //Quaternion D_Q = Quaternion::from_euler_rotation_approx(dalpha*DEGtoRAD, dbeta*DEGtoRAD, dgamma*DEGtoRAD);

        // Nummeric Integration - time increment
        // q.normalize();
        //q += (q*D_Q)*dt;
        // always normalize

        // --------Approach B
        // In testing it works even better for time integration ... more stable approach !!
        q *= Quaternion::from_euler_rotation_approx(dalpha*dt*DEGtoRAD, dbeta*dt*DEGtoRAD, dgamma*dt*DEGtoRAD);


        if(counter % 5 == 0)
        {
            Quaternion q2 = q;

            q2.normalize();
            // Get Euler angels from Quaternion
            EulerAngles AA = q.ToEulerAngles();
            // Clear Display
            gOled2.clearDisplay();
            gOled2.setTextCursor(0, 2);
            gOled2.printf("Psi:    %.2f \n\r" , AA.yaw*RADtoDEG);
            gOled2.printf("Theta:  %.2f \n\r" , AA.pitch*RADtoDEG);
            gOled2.printf("Phi:    %.2f \n\r" , AA.roll*RADtoDEG);
            gOled2.printf("%u \n\r" , counter);

            gOled2.printf("dt: %f \n\r" , dt);


            // very basic compass animation for yaw
            int offx = 100, offy = 32;
            float r= 10;
            float a = AA.yaw;
            float c = cos(a);
            float s = sin(a);

            gOled2.drawLine(offx+r*s, offy+c*r, offx-s*r, offy-c*r, 1);
            gOled2.drawLine(offx+r*s+c*3, offy+c*r-s*3, offx+s*r-c*3, offy+c*r+s*3, 1);

            gOled2.display();

            // Send Data via Serial Port --- Sending Unit Quaternion directly
            serialPc.SendDouble(1, q2.a);
            serialPc.SendDouble(2, q2.b);
            serialPc.SendDouble(3, q2.c);
            serialPc.SendDouble(4, q2.d);

        }
        // Print to Oled Screen


        // increment
        counter++;


        // This calculation will lead to gimbal lock!
        // Alternative calculation  (f = PI / 180°) + timeintegration Euler explicit (phi += dphi* dt )
        //double f = 3.14159265/180.0d;
        //phi += (dbeta*sin(phi*f)+dgamma*cos(phi*f))/cos(theta*f)*dt;
        //theta += (dbeta*cos(phi*f)-dgamma*sin(phi*f))*dt;
        //psi += (dalpha+(dbeta*sin(phi*f)+dgamma*cos(phi*f))*tan(theta*f))*dt;

    }
}