Skip to content

Commit e9e6280

Browse files
vpalmeriovpalmerio
committed
Added sys id code with IMU integration
1 parent 958b216 commit e9e6280

File tree

6 files changed

+561
-0
lines changed

6 files changed

+561
-0
lines changed

‎.gitignore

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
.DS_Store

‎SYSID_IMU/Error.h

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
/*
2+
Error.h - Error Header/Constants File
3+
Description: Error Constants for General Errors that can occur throughout Astra
4+
Author: Vincent Palmerio
5+
*/
6+
7+
#ifndef ERROR_H
8+
#define ERROR_H
9+
10+
#define NO_ERROR_CODE (0)
11+
#define GENERAL_ERROR_CODE (-1)
12+
#define MEMORY_ALLOCATION_ERROR_CODE (-2)
13+
#define VECTOR_INIT_ZERO_SIZE_ERROR (-3) // size of vector cannot be 0
14+
#define MAX_ALLOWED_TIME_BETWEEN_INTEGRATION_EXCEEDED (-4)
15+
#define VECTOR_SIZE_MISMATCH (-5)
16+
17+
#define INNER_GIMBAL_NOT_ATTACHED (-6)
18+
#define OUTER_GIMBAL_NOT_ATTACHED (-7)
19+
#define LEFT_TORQUE_VANE_NOT_ATTACHED (-8)
20+
#define RIGHT_TORQUE_VANE_NOT_ATTACHED (-9)
21+
#define ESC_NOT_ATTACHED (-14)
22+
23+
#define ROW_KGAIN_MISMATCH (-10)
24+
#define COLUMN_KGAIN_MISMATCH (-11)
25+
#define COLUMN_QSM_MISMATCH (-12)
26+
27+
#define CANNOT_REQUEST_MISSION_TIME_IF_MISSION_NOT_STARTED (-13)
28+
29+
#define INVALID_ESC_BUFFER_INIT (-15)
30+
#define INVALID_THROTTLE_ESC (-16)
31+
32+
#define FAILED_CALIBRATION_HELPER_INIT (-100)
33+
#define FAILED_LOAD_CALIBRATION (-101)
34+
#define FAILED_SENSOR_INIT (-102)
35+
36+
#endif

‎SYSID_IMU/IMU.cpp

+200
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,200 @@
1+
#include "Error.h"
2+
#include "IMU.h"
3+
#include "LSM6DS_LIS3MDL.h"
4+
5+
/*
6+
IMU.cpp
7+
Description: Function definititions for declarations in IMU.h
8+
Author: Vincent Palmerio
9+
10+
*/
11+
12+
13+
Eigen::VectorXd linearAccelVector(3);
14+
float linearAccelX, linearAccelY, linearAccelZ = 0;
15+
Eigen::VectorXd gyroscopeVector(3);
16+
Eigen::VectorXd magnetometerVector(3);
17+
float roll, pitch, yaw = 0;
18+
float gx, gy, gz = 0; //degrees per second on gyro
19+
float qw, qx, qy, qz = 0; //quaternarion
20+
21+
/*
22+
* You must free the pointer and set it to NULL after using the pointer!
23+
*/
24+
float* getValues() {
25+
float* values = (float*)malloc(3 * sizeof(float));
26+
values[0] = roll;
27+
values[1] = pitch;
28+
values[2] = yaw;
29+
return values;
30+
}
31+
32+
33+
//loads a predetermined calibration into the EEPROM
34+
int loadPresetCalibration() {
35+
//Magnetic Hard Offset
36+
cal.mag_hardiron[0] = -3.35;
37+
cal.mag_hardiron[1] = -0.74;
38+
cal.mag_hardiron[2] = -40.79;
39+
40+
//Magnetic Soft Offset
41+
// in uTesla
42+
cal.mag_softiron[0] = 0.96;
43+
cal.mag_softiron[1] = 0.02;
44+
cal.mag_softiron[2] = 0.01;
45+
cal.mag_softiron[3] = 0.02;
46+
cal.mag_softiron[4] = 0.96;
47+
cal.mag_softiron[5] = 0.0;
48+
cal.mag_softiron[6] = 0.01;
49+
cal.mag_softiron[7] = 0.0;
50+
cal.mag_softiron[8] = 1.08;
51+
52+
//Gyro zero rate offset
53+
// in Radians/s
54+
cal.gyro_zerorate[0] = 0.05;
55+
cal.gyro_zerorate[1] = -0.01;
56+
cal.gyro_zerorate[2] = -0.01;
57+
58+
if (cal.saveCalibration()) {
59+
return FAILED_LOAD_CALIBRATION;
60+
}
61+
62+
return NO_ERROR_CODE;
63+
}
64+
65+
66+
67+
int initializeIMU() {
68+
//Serial.begin(115200);
69+
//while (!Serial) yield();
70+
//Serial.println("test");
71+
for (int i = 0; i < linearAccelVector.size(); i++) {
72+
linearAccelVector(i) = 0;
73+
}
74+
75+
76+
if (!cal.begin()) {
77+
//Failed to initialize calibration helper
78+
#if defined(ASTRA_FULL_DEBUG) or defined(ASTRA_IMU_DEBUG)
79+
Serial.print("Failed to initialize calibration helper");
80+
#endif
81+
return FAILED_CALIBRATION_HELPER_INIT;
82+
}
83+
84+
if (!cal.loadCalibration()) {
85+
//No calibration loaded/found
86+
static bool triedLoadPresetCalibration = false;
87+
if (!triedLoadPresetCalibration) {
88+
if (loadPresetCalibration() == FAILED_LOAD_CALIBRATION) {
89+
return FAILED_LOAD_CALIBRATION;
90+
}
91+
}
92+
#if defined(ASTRA_FULL_DEBUG) or defined(ASTRA_IMU_DEBUG)
93+
Serial.println("Failed to load calibration");
94+
#endif
95+
96+
}
97+
98+
if (!init_sensors(&IMU_WIRE)) {
99+
//Failed to find sensors
100+
#if defined(ASTRA_FULL_DEBUG) or defined(ASTRA_IMU_DEBUG)
101+
Serial.println("Failed to find sensors");
102+
#endif
103+
return FAILED_SENSOR_INIT;
104+
}
105+
106+
#if defined(ASTRA_FULL_DEBUG) or defined(ASTRA_IMU_DEBUG)
107+
accelerometer->printSensorDetails();
108+
gyroscope->printSensorDetails();
109+
magnetometer->printSensorDetails();
110+
#endif
111+
112+
setup_sensors();
113+
filter.begin(FILTER_UPDATE_RATE_HZ);
114+
115+
Wire.setClock(400000); // 400KHz
116+
117+
return NO_ERROR_CODE;
118+
}
119+
120+
//sets varaibles declared in IMU.h based on latest data IMU
121+
int updateIMU() {
122+
123+
// Read the motion sensors
124+
sensors_event_t accel, gyro, mag;
125+
accelerometer->getEvent(&accel);
126+
gyroscope->getEvent(&gyro);
127+
magnetometer->getEvent(&mag);
128+
129+
cal.calibrate(mag);
130+
cal.calibrate(accel);
131+
cal.calibrate(gyro);
132+
133+
// Gyroscope needs to be converted from Rad/s to Degree/s
134+
// the rest are not unit-important
135+
136+
gx = gyro.gyro.x; //* SENSORS_RADS_TO_DPS; //omega x
137+
gy = gyro.gyro.y; //* SENSORS_RADS_TO_DPS; //omega y
138+
gz = gyro.gyro.z; //* SENSORS_RADS_TO_DPS; //omega z
139+
140+
// Update the SensorFusion filter
141+
filter.update(gx, gy, gz,
142+
accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
143+
mag.magnetic.x, mag.magnetic.y, mag.magnetic.z);
144+
145+
// print the heading, pitch and roll
146+
roll = filter.getRoll();
147+
pitch = filter.getPitch();
148+
yaw = filter.getYaw();
149+
150+
151+
//float qw, qx, qy, qz;
152+
filter.getQuaternion(&qw, &qx, &qy, &qz);
153+
154+
155+
filter.getLinearAcceleration(&linearAccelX, &linearAccelY, &linearAccelZ); //"a" - linear acceleration
156+
157+
linearAccelVector << linearAccelX, linearAccelY, linearAccelZ;
158+
gyroscopeVector << gx, gy, gz;
159+
magnetometerVector << mag.magnetic.x, mag.magnetic.y, mag.magnetic.z;
160+
161+
#if defined(ASTRA_FULL_DEBUG) or defined(ASTRA_IMU_DEBUG)
162+
163+
//Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
164+
165+
//Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
166+
Serial.print("Raw: ");
167+
Serial.print(accel.acceleration.x, 4); Serial.print(", ");
168+
Serial.print(accel.acceleration.y, 4); Serial.print(", ");
169+
Serial.print(accel.acceleration.z, 4); Serial.print(", ");
170+
Serial.print(gx, 4); Serial.print(", ");
171+
Serial.print(gy, 4); Serial.print(", ");
172+
Serial.print(gz, 4); Serial.print(", ");
173+
Serial.print(mag.magnetic.x, 4); Serial.print(", ");
174+
Serial.print(mag.magnetic.y, 4); Serial.print(", ");
175+
Serial.print(mag.magnetic.z, 4); Serial.println("");
176+
177+
Serial.print("Orientation: ");
178+
Serial.print(heading);
179+
Serial.print(", ");
180+
Serial.print(pitch);
181+
Serial.print(", ");
182+
Serial.println(roll);
183+
184+
Serial.print("Quaternion: ");
185+
Serial.print(qw, 4);
186+
Serial.print(", ");
187+
Serial.print(qx, 4);
188+
Serial.print(", ");
189+
Serial.print(qy, 4);
190+
Serial.print(", ");
191+
Serial.println(qz, 4);
192+
//Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms");
193+
194+
#endif
195+
196+
197+
return NO_ERROR_CODE;
198+
199+
}
200+

‎SYSID_IMU/IMU.h

+75
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
2+
/*
3+
IMU.h - Header file for the IMU Driver
4+
Description: Contains all functions and variables to interact with the IMU
5+
Author: Vincent Palmerio
6+
7+
Last updated: 11/4/2023
8+
9+
*/
10+
11+
#ifndef IMU_H
12+
#define IMU_H
13+
14+
#include <Adafruit_LSM6DSOX.h>
15+
#include <Adafruit_LIS3MDL.h>
16+
#include <Adafruit_AHRS.h>
17+
#include <Adafruit_Sensor_Calibration.h>
18+
#include <Adafruit_AHRS.h>
19+
#include <ArduinoEigenDense.h>
20+
#include <Wire.h>
21+
22+
//uncomment to print data to console for just IMU
23+
//#define ASTRA_IMU_DEBUG
24+
25+
26+
27+
// Full orientation sensing using NXP/Madgwick/Mahony and a range of 9-DoF
28+
// sensor sets.
29+
// You *must* perform a magnetic calibration before this code will work.
30+
//
31+
// To view this data, use the Arduino Serial Monitor to watch the
32+
// scrolling angles, or run the OrientationVisualiser example in Processing.
33+
// Based on https://github.com/PaulStoffregen/NXPMotionSense with adjustments
34+
// to Adafruit Unified Sensor interface
35+
36+
37+
38+
#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM)
39+
static Adafruit_Sensor_Calibration_EEPROM cal;
40+
#else
41+
Adafruit_Sensor_Calibration_SDFat cal;
42+
#endif
43+
44+
#define FILTER_UPDATE_RATE_HZ (100)
45+
#define PRINT_EVERY_N_UPDATES (10)
46+
#define IMU_WIRE Wire
47+
//#define AHRS_DEBUG_OUTPUT
48+
49+
50+
extern float* values;
51+
extern float roll, pitch, yaw;
52+
extern Eigen::VectorXd linearAccelVector;
53+
extern float linearAccelX, linearAccelY, linearAccelZ;
54+
55+
extern Eigen::VectorXd gyroscopeVector;
56+
extern Eigen::VectorXd magnetometerVector;
57+
58+
extern float gx, gy, gz; //degrees per second on gyro
59+
extern float qw, qx, qy, qz; //quaternarion
60+
61+
// slower == better quality output
62+
static Adafruit_NXPSensorFusion filter; // slowest
63+
64+
65+
extern int initializeIMU();
66+
extern int updateIMU();
67+
extern int loadPresetCalibration();
68+
extern float* getValues();
69+
70+
71+
#endif
72+
73+
74+
75+

‎SYSID_IMU/LSM6DS_LIS3MDL.h

+61
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
2+
/*
3+
LSM6DS_LIS3MDL.h
4+
Description: Header file for sensor-setup function definitions
5+
Author: Vincent Palmerio, (Original: https://github.com/adafruit/Adafruit_AHRS/blob/master/examples/calibrated_orientation/calibrated_orientation.ino)
6+
7+
8+
Last updated: 11/4/2023
9+
10+
*/
11+
12+
#ifndef LSM6DS_LIS3MDL
13+
#define LSM6DS_LIS3MDL
14+
15+
#include <Adafruit_LIS3MDL.h>
16+
Adafruit_LIS3MDL lis3mdl;
17+
18+
#include <Adafruit_LSM6DSOX.h>
19+
Adafruit_LSM6DSOX lsm6ds;
20+
21+
#include <Wire.h>
22+
23+
Adafruit_Sensor* accelerometer;
24+
Adafruit_Sensor* gyroscope;
25+
Adafruit_LIS3MDL* magnetometer;
26+
27+
bool init_sensors(TwoWire *i2c_wire) {
28+
29+
//See https://www.adafruit.com/product/4517 for possible I2C addresses for each device
30+
if (!lsm6ds.begin_I2C() && !lsm6ds.begin_I2C((uint8_t) 0x6A, i2c_wire, 0L) && !lsm6ds.begin_I2C((uint8_t) 0x6B, i2c_wire, 0L)) {
31+
return false;
32+
}
33+
if (!lis3mdl.begin_I2C((uint8_t) 0x1C, i2c_wire)) {
34+
lis3mdl = *(new Adafruit_LIS3MDL);
35+
if (!lis3mdl.begin_I2C((uint8_t) 0x1E, i2c_wire)) {
36+
return false;
37+
}
38+
}
39+
40+
accelerometer = lsm6ds.getAccelerometerSensor();
41+
gyroscope = lsm6ds.getGyroSensor();
42+
magnetometer = &lis3mdl;
43+
44+
return true;
45+
}
46+
47+
void setup_sensors(void) {
48+
// set lowest range
49+
lsm6ds.setAccelRange(LSM6DS_ACCEL_RANGE_2_G);
50+
lsm6ds.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
51+
lis3mdl.setRange(LIS3MDL_RANGE_12_GAUSS);
52+
53+
// set slightly above refresh rate
54+
lsm6ds.setAccelDataRate(LSM6DS_RATE_3_33K_HZ);
55+
lsm6ds.setGyroDataRate(LSM6DS_RATE_3_33K_HZ);
56+
lis3mdl.setDataRate(LIS3MDL_DATARATE_5_HZ);
57+
lis3mdl.setPerformanceMode(LIS3MDL_ULTRAHIGHMODE);
58+
lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE);
59+
}
60+
61+
#endif

0 commit comments

Comments
 (0)