Rocket-Flight-Software-and-State-Machine/NineDOF.cpp at main · matthewchen132/Rocket-Flight-Software-and-State-Machine · 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
155
156
157
158
159
160
#include <Adafruit_ICM20948.h>
#include "NineDOF.h"

#define LSM_CS 38
#define LSM_SCK 37
#define LSM_MISO 36
#define LSM_MOSI 35

Adafruit_ICM20948 icm;

bool IGNITABLE = false;
double Net_Accel;
const double GRAVITY = 9.81;
double deltaTime = 0.01;

bool apogeeReached = false;
double vertical_velocity = 0.0;
double vertical_position = 0.0;
double previous_vertical_velocity = 0.0;
double previous_altitude = 0.0;

double velocity_x = 0.0;
double velocity_y = 0.0;
double velocity_z = 0.0;

double position_x = 0.0;
double position_y = 0.0;
double position_z = 0.0;

NineDOF::NineDOF()
{
}

Adafruit_ICM20948 icm = Adafruit_ICM20948();

// Function definition
bool NineDOF::start_9DOF()
{
    Serial.println("Adafruit ICM20948 test!");
    if (!icm.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI))
    {
        Serial.println("Failed to find ICM20948 chip");
        return (false);
    }
    Serial.println("ICM20948 Found!");
    return true;
}

// Optional function definition (if needed in SixDOF.h)
String NineDOF::printSensorData()
{
    sensors_event_t accel1;
    sensors_event_t gyro1;
    sensors_event_t temp1;
    sensors_event_t mag1;
    icm.getEvent(&accel1, &gyro1, &temp1, &mag1);
    double Ax = accel1.acceleration.x;
    double Ay = accel1.acceleration.y;
    double Az = accel1.acceleration.z;
    Net_Accel = sqrt((pow(Ax, 2) + pow(Ay, 2) + pow(Az, 2)));
    return "(9DOF)" + String(Ax) + "," + String(Ay) + "," + String(Az) + "," + String(gyro1.gyro.x) + "," + String(gyro1.gyro.y) + "," + String(gyro1.gyro.z) + "," + String(mag1.magnetic.x) + "," + String(mag1.magnetic.y) + "," + String(mag1.magnetic.z) + "\n";
}

vector<double> NineDOF::getAcceleration()
{
    sensors_event_t accel1;
    sensors_event_t gyro1;
    sensors_event_t temp1;
    sensors_event_t mag1;
    icm.getEvent(&accel1, &gyro1, &temp1, &mag1);
    return {accel1.acceleration.x, accel1.acceleration.y, accel1.acceleration.z};
}
vector<double> NineDOF::getMag()
{
    sensors_event_t accel1;
    sensors_event_t gyro1;
    sensors_event_t temp1;
    sensors_event_t mag1;
    icm.getEvent(&accel1, &gyro1, &temp1, &mag1);
    return {mag1.magnetic.x, mag1.magnetic.y, mag1.magnetic.z};
}

vector<double> NineDOF::getGyro()
{
    sensors_event_t gyro1;
    return {static_cast<double>(gyro1.gyro.x), static_cast<double>(gyro1.gyro.y), static_cast<double>(gyro1.gyro.z)};
}

bool NineDOF::_init(int32_t sensor_id)
{
    return true; // Example return value, modify as needed
}

bool NineDOF::check_IGNITABLE()
{ // is there a way to use switch-cases here? Idk how to make cases for all values >10
    Serial.print("    Net Acceleration: " + String(Net_Accel) + ", ");
    if (Net_Accel > 68.6)
    {
        IGNITABLE = true;
    }
    else
    {
        switch (int(Net_Accel))
        {
        default:
            return IGNITABLE;
            break;
        }
    }
    return IGNITABLE;
}
bool NineDOF::checkReadings()
{
    vector<double> accelData = getAcceleration();
    vector<double> gyroData = getGyro();
    for (int i = 0; i < accelData.size(); i++)
    {
        if (accelData[i] != 0)
        {
            return true;
        }
    }
    for (int i = 0; i < gyroData.size(); i++)
    {
        if (gyroData[i] != 0)
        {
            return true;
        }
    }
    return false;
}

// need to call manually in main.cpp file
// void SixDOF::updateQuaternionFilter()
// {
//   vector<double> accelData = getAcceleration();
//   vector<double> gyroData = getGyro();

//   double gyroX = gyroData[0];
//   double gyroY = gyroData[1];
//   double gyroZ = gyroData[2];

//   double accelX = accelData[0];
//   double accelY = accelData[1];
//   double accelZ = accelData[2];

//   filter.update(accelX, accelY, accelZ, gyroX, gyroY, gyroZ, 0, 0, 0, quaternion);
// }


vector<double> NineDOF::getPositions()
{
    return {position_x, position_y, position_z};
}

vector<double> NineDOF::getVelocities()
{
    return {velocity_x, velocity_y, velocity_z};
}