-
Notifications
You must be signed in to change notification settings - Fork 1
/
main.cpp
155 lines (132 loc) · 4.09 KB
/
main.cpp
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
#include "HAL/hal.h"
#include "libs/myLib.h"
#include "icm20948/icm20948.h"
#include "serialPort/uartHW.h"
#include <cstdio>
#include "driverlib/interrupt.h"
#include "driverlib/systick.h"
static float timestamp = 0;
void SysTickIntHandler(void)
{
// Update the Systick interrupt counter.
timestamp += 100.0;
}
// DEfinition of these functions for deployment platform must exist
extern "C" {
/*
* Sleep implementation for ICM20948
*/
void inv_icm20948_sleep(int ms) {
HAL_DelayUS(ms*1000);
}
void inv_icm20948_sleep_us(int us){
HAL_DelayUS(us);
}
uint64_t inv_icm20948_get_time_us(void){
return timestamp;
}
}
/**
* main.cpp
*/
int main(void)
{
int8_t rc = 0;
// Mounting matrix describing transformation between IMU reference frame
// and a mounting pose
float mountMatrix[9]= {
1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
};
ICM20948& imu = ICM20948::GetI();
// Initialize board and FPU
HAL_BOARD_CLOCK_Init();
SysTickPeriodSet(12000);
SysTickIntRegister(SysTickIntHandler);
SysTickIntEnable();
SysTickEnable();
// Initialize serial port
SerialPort::GetI().InitHW();
DEBUG_WRITE("Initialized Uart... \n");
// Initialize hardware used by the IMU
imu.InitHW();
// Set instrument settings
imu.SetAccelerationFSR(AccelFSR2g);
imu.SetGyroscopeFSR(GyroFSR250dps);
imu.SetMagnetometerBias(-73.363101, -69.95, -3.0);
imu.SetMountingMatrix(mountMatrix);
// Software initialization of the IMU
// (load DMP firmware and enable all the sensors)
imu.InitSW();
//
// DMP has been loaded, enable the sensors
//
rc = imu.EnableSensor(INV_ICM20948_SENSOR_GYROSCOPE, 5);
#ifdef __DEBUG_SESSION__
if (rc == 0)
DEBUG_WRITE("OK\n");
else
DEBUG_WRITE("ERR\n");
#endif
// 6DOF sensor fusion
rc = imu.EnableSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR, 5);
#ifdef __DEBUG_SESSION__
if (rc == 0)
DEBUG_WRITE("OK\n");
else
DEBUG_WRITE("ERR\n");
DEBUG_WRITE(" Linear acceleration: ");
#endif
rc = imu.EnableSensor(INV_ICM20948_SENSOR_LINEAR_ACCELERATION, 5);
#ifdef __DEBUG_SESSION__
if (rc == 0)
DEBUG_WRITE("OK\n");
else
DEBUG_WRITE("ERR\n");
DEBUG_WRITE(" 9DOF fusion: ");
#endif
// 9DOF sensor fusion
rc = imu.EnableSensor(INV_ICM20948_SENSOR_ROTATION_VECTOR, 5);
#ifdef __DEBUG_SESSION__
if (rc == 0)
DEBUG_WRITE("OK\n");
else
DEBUG_WRITE("ERR\n");
DEBUG_WRITE(" Gravity vector: ");
#endif
rc = imu.EnableSensor(INV_ICM20948_SENSOR_GRAVITY, 5);
#ifdef __DEBUG_SESSION__
if (rc == 0)
DEBUG_WRITE("OK\n");
else
DEBUG_WRITE("ERR\n");
DEBUG_WRITE(" Gravity vector: ");
#endif
float data[3];
while (1)
{
// Check if IMU toggled interrupt pin
// (this example doesn't use interrupts, but polling)
if (imu.IsDataReady())
{
char buffer[160];
// Read sensor data
imu.ReadSensorData(timestamp);
// Read and print orientation as reported by the 6DOF and 9DOF fusion
imu.GetOrientationRPY(Orientation6DOF, data, true);
snprintf(buffer, 160, "%f,%f,%f",data[0],data[1],data[2]);
imu.GetOrientationRPY(Orientation9DOF, data, true);
snprintf(buffer, 160, "%s,%f,%f,%f",buffer, data[0],data[1],data[2]);
DEBUG_WRITE("%s\n", buffer);
// Read acceleration and angular velocity
imu.GetLinearAcceleration(data);
snprintf(buffer, 160, "%f,%f,%f",data[0],data[1],data[2]);
imu.GetGyroscope(data);
snprintf(buffer, 160, "%s,%f,%f,%f",buffer, data[0],data[1],data[2]);
DEBUG_WRITE("%s\n\n", buffer);
}
// INT pin can be held up for max 50us, so delay here to prevent reading the same data twice
HAL_DelayUS(50);
}
}