JeVois  1.21
JeVois Smart Embedded Machine Vision Toolkit
Share this page:
Loading...
Searching...
No Matches
IMUdata.H
Go to the documentation of this file.
1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2//
3// JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2018 by Laurent Itti, the University of Southern
4// California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5//
6// This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7// redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8// Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9// without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10// License for more details. You should have received a copy of the GNU General Public License along with this program;
11// if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12//
13// Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14// Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16/*! \file */
17
18#pragma once
19
20#include <vector>
21#include <string>
22
23namespace jevois
24{
25 /*! \defgroup imu Inertial Measurement Unit (IMU) classes and functions
26 \ingroup core
27
28 These classes and functions provide support for optional IMU chips that may be present in some of the optional
29 JeVois imaging sensors. For example, TDK InvenSense ICM20948: 3-axis accelerometer, 3-axis gyroscope, 3-axis
30 magnetometer, integrated digital motion processor (DMP). */
31
32 //! Raw IMU data
33 /*! These are raw measurements from the sensor. Note that they are unscaled and thus their meaning depends on the
34 selected scales for the sensors (e.g., +/-2g or +/-16g for the accelerometer). Use IMUdata for scaled
35 measurements. \ingroup imu */
37 {
38 short v[11]; //!< The values: ax, ay, az, gy, gy, gz, temp, mx, my, mz, mst2
39
40 inline short & ax() { return v[0]; }
41 inline short const & ax() const { return v[0]; }
42 inline short & ay() { return v[1]; }
43 inline short const & ay() const { return v[1]; }
44 inline short & az() { return v[2]; }
45 inline short const & az() const { return v[2]; }
46 inline short & gx() { return v[3]; }
47 inline short const & gx() const { return v[3]; }
48 inline short & gy() { return v[4]; }
49 inline short const & gy() const { return v[4]; }
50 inline short & gz() { return v[5]; }
51 inline short const & gz() const { return v[5]; }
52 inline short & temp() { return v[6]; }
53 inline short const & temp() const { return v[6]; }
54 inline short & mx() { return v[7]; }
55 inline short const & mx() const { return v[7]; }
56 inline short & my() { return v[8]; }
57 inline short const & my() const { return v[8]; }
58 inline short & mz() { return v[9]; }
59 inline short const & mz() const { return v[9]; }
60 inline short & mst2() { return v[10]; }
61 inline short const & mst2() const { return v[10]; }
62 };
63
64 //! IMU data
65 /*! These are scaled measurements from the sensor, in absolute units (g for accelerations, degrees per second (dps)
66 for gyro, degrees Celsius for temperature, and micro-Tesla for magnetometer). \ingroup imu */
67 struct IMUdata
68 {
69 //! Construct from a raw data reading
70 IMUdata(IMUrawData const & rd, double arange, double grange);
71
72 float v[10]; //!< The values: ax, ay, az, gy, gy, gz, temp, mx, my, mz
73 bool magovf; //!< True if magnetometer overflow
74
75 inline float & ax() { return v[0]; }
76 inline float const & ax() const { return v[0]; }
77 inline float & ay() { return v[1]; }
78 inline float const & ay() const { return v[1]; }
79 inline float & az() { return v[2]; }
80 inline float const & az() const { return v[2]; }
81 inline float & gx() { return v[3]; }
82 inline float const & gx() const { return v[3]; }
83 inline float & gy() { return v[4]; }
84 inline float const & gy() const { return v[4]; }
85 inline float & gz() { return v[5]; }
86 inline float const & gz() const { return v[5]; }
87 inline float & temp() { return v[6]; }
88 inline float const & temp() const { return v[6]; }
89 inline float & mx() { return v[7]; }
90 inline float const & mx() const { return v[7]; }
91 inline float & my() { return v[8]; }
92 inline float const & my() const { return v[8]; }
93 inline float & mz() { return v[9]; }
94 inline float const & mz() const { return v[9]; }
95 };
96
97 /*! \defgroup icmdmpheader ICM-20948 DMP computation header fields
98 \ingroup imu */
99 /*! @{ */
100#define JEVOIS_DMP_ACCEL 0x8000 /*!< calibrated accel if accel calibrated, raw accel otherwise */
101#define JEVOIS_DMP_GYRO 0x4000 /*!< raw gyro */
102#define JEVOIS_DMP_CPASS 0x2000 /*!< raw magnetic */
103#define JEVOIS_DMP_ALS 0x1000 /*!< ALS/proximity */
104#define JEVOIS_DMP_QUAT6 0x0800 /*!< game rotation vector */
105#define JEVOIS_DMP_QUAT9 0x0400 /*!< rotation vector with heading accuracy */
106#define JEVOIS_DMP_PQUAT6 0x0200 /*!< truncated game rotation vector for batching */
107#define JEVOIS_DMP_GEOMAG 0x0100 /*!< geomag rotation vector with heading accuracy */
108#define JEVOIS_DMP_PRESSURE 0x0080 /*!< pressure */
109#define JEVOIS_DMP_GYRO_CALIBR 0x0040 /*!< calibrated gyro */
110#define JEVOIS_DMP_CPASS_CALIBR 0x0020 /*!< calibrated magnetic */
111#define JEVOIS_DMP_PED_STEPDET 0x0010 /*!< timestamp when each step is detected */
112#define JEVOIS_DMP_HEADER2 0x0008 /*!< enable/disable data output in data output control register 2 */
113#define JEVOIS_DMP_PED_STEPIND 0x0007 /*!< number of steps detected is in 3 LSBs of header (JEVOIS: always 0...) */
114
115#define JEVOIS_DMP_ACCEL_ACCURACY 0x4000 /*!< accel accuracy when changes (HEADER2) */
116#define JEVOIS_DMP_GYRO_ACCURACY 0x2000 /*!< gyro accuracy when changes (HEADER2) */
117#define JEVOIS_DMP_CPASS_ACCURACY 0x1000 /*!< compass accuracy when changes (HEADER2) */
118#define JEVOIS_DMP_FSYNC 0x0800 /*!< frame sync from camera sensor (HEADER2) */
119#define JEVOIS_DMP_FLIP_PICKUP 0x0400 /*!< Flip/pick-up gesture detector (HEADER2) */
120#define JEVOIS_DMP_BATCH_MODE_EN 0x0100 /*!< enable batching (HEADER2) */
121#define JEVOIS_DMP_ACT_RECOG 0x0080 /*!< Activity recognition engine (HEADER2)*/
122 /*! @} */
123
124 //! DMP data (Digital Motion Processor)
125 /*! The digital motion processor (DMP) inside the ICM-20948 IMU chip can compute a variety of things that range from
126 simply the raw data buffered at a fixed rate to quaternions to activity detection (footsteps, driving, biking,
127 etc). The chip must be operating in DMP mode for this data to be available. In addition, one can configure which
128 data is computed and output by the DMP. \ingroup imu */
129 struct DMPdata
130 {
131 //! Populate our fields from a packet received from the DMP
132 void parsePacket(unsigned char const * packet, size_t siz);
133
134 unsigned short header1; //!< Header 1 fields that indicate what data is valid
135 unsigned short header2; //!< Header 2 fields that indicate what data is valid
136
137 short accel[3]; //!< Raw accelerometer data (when JEVOIS_DMP_ACCEL in header1)
138 short gyro[3]; //!< Raw gyro data (when JEVOIS_DMP_GYRO in header1)
139 short cpass[3]; //!< Raw compass data (when JEVOIS_DMP_CPASS in header1)
140 short gbias[3]; //!< Raw gyro bias data (when JEVOIS_DMP_GYRO in header1)
141
142 long quat6[3]; //!< Quaternion6 data (when JEVOIS_DMP_QUAT6 in header1)
143 long quat9[3]; //!< Quaternion9 data (when JEVOIS_DMP_QUAT9 in header1)
144 long quat9acc; //!< Quaternion9 accuracy (when JEVOIS_DMP_QUAT9 in header1)
145
146 unsigned long stepts; //!< Step detection timestamp (when JEVOIS_DMP_PED_STEPDET)
147 unsigned short steps; //!< Number of steps (0..7) detected this cycle (when JEVOIS_DMP_PED_STEPDET)
148
149 long geomag[3]; //!< Geomag data (when JEVOIS_DMP_GEOMAG in header1)
150 long geomagacc; //!< Geomag accuracy (when JEVOIS_DMP_GEOMAG in header1)
151
152 short gyrobias[3]; //!< Gyro bias/calibration data (when JEVOIS_DMP_GYRO_CALIBR in header1)
153 long cpasscal[3]; //!< Compass calibration data (when JEVOIS_DMP_CPASS_CALIBR in header1)
154
155 short accelacc; //!< Accelerometer accuracy data (when JEVOIS_DMP_ACCEL_ACCURACY in header2)
156 short gyroacc; //!< Gyro accuracy data (when JEVOIS_DMP_GYRO_ACCURACY in header2)
157 short cpassacc; //!< Compass accuracy data (when JEVOIS_DMP_CPASS_ACCURACY in header2)
158
159 unsigned short fsync; //!< Delay between FSYNC received from camera and first subsequent IMU data generated
160 float fsync_us() const; //!< Delay between FSYNC and next IMU data, in microseconds
161
162 short pickup; //!< Flip/pickup detection (when JEVOIS_DMP_FLIP_PICKUP in header2)
163
164 unsigned short bacstate; //!< Activity recognition state (when JEVOIS_DMP_ACT_RECOG in header2)
165 long bacts; //!< Activity recognition timestamp (when JEVOIS_DMP_ACT_RECOG in header2)
166
167 unsigned short odrcnt; //!< Output data rate counter (always here but unclear what it is)
168
169 //!< Decode start/stop of activities into a string
170 /*! Requires that JEVOIS_DMP_ACT_RECOG be set in header2 and correct data rates are used. This function returns
171 strings that contain start or stop as a prefix, then each activity. Activities are: drive, walk, run, bike,
172 tilt, still. */
173 std::vector<std::string> activity();
174
175 //!< Decode current ongoing activities into a string
176 /*! Requires that JEVOIS_DMP_ACT_RECOG be set in header2 and correct data rates are used. This function returns
177 strings that contain the name of each ongoing event. Warning, it has state, so you should only call this
178 function for any activity-related parsing after you start using it, and you should call it for every reading
179 from the DMP. Activities are: drive, walk, run, bike, tilt, still. */
180 std::vector<std::string> activity2();
181
182 //! Convert a long fixed-point value to float
183 static float fix2float(long val);
184};
185
186 //! Helper function to determine DMP packet size depending on options
187 /*! \ingroup imu */
188 size_t DMPpacketSize(unsigned short ctl1, unsigned short ctl2);
189};
size_t DMPpacketSize(unsigned short ctl1, unsigned short ctl2)
Helper function to determine DMP packet size depending on options.
Definition IMUdata.C:52
Main namespace for all JeVois classes and functions.
Definition Concepts.dox:2
DMP data (Digital Motion Processor)
Definition IMUdata.H:130
short cpassacc
Compass accuracy data (when JEVOIS_DMP_CPASS_ACCURACY in header2)
Definition IMUdata.H:157
unsigned short bacstate
Activity recognition state (when JEVOIS_DMP_ACT_RECOG in header2)
Definition IMUdata.H:164
short gyroacc
Gyro accuracy data (when JEVOIS_DMP_GYRO_ACCURACY in header2)
Definition IMUdata.H:156
unsigned short steps
Number of steps (0..7) detected this cycle (when JEVOIS_DMP_PED_STEPDET)
Definition IMUdata.H:147
long quat9acc
Quaternion9 accuracy (when JEVOIS_DMP_QUAT9 in header1)
Definition IMUdata.H:144
long cpasscal[3]
Compass calibration data (when JEVOIS_DMP_CPASS_CALIBR in header1)
Definition IMUdata.H:153
short gyro[3]
Raw gyro data (when JEVOIS_DMP_GYRO in header1)
Definition IMUdata.H:138
short gyrobias[3]
Gyro bias/calibration data (when JEVOIS_DMP_GYRO_CALIBR in header1)
Definition IMUdata.H:152
std::vector< std::string > activity2()
Definition IMUdata.C:259
unsigned short fsync
Delay between FSYNC received from camera and first subsequent IMU data generated.
Definition IMUdata.H:159
short accel[3]
Raw accelerometer data (when JEVOIS_DMP_ACCEL in header1)
Definition IMUdata.H:137
unsigned short header1
Header 1 fields that indicate what data is valid.
Definition IMUdata.H:134
short accelacc
Accelerometer accuracy data (when JEVOIS_DMP_ACCEL_ACCURACY in header2)
Definition IMUdata.H:155
void parsePacket(unsigned char const *packet, size_t siz)
Populate our fields from a packet received from the DMP.
Definition IMUdata.C:84
short cpass[3]
Raw compass data (when JEVOIS_DMP_CPASS in header1)
Definition IMUdata.H:139
std::vector< std::string > activity()
Decode current ongoing activities into a string.
Definition IMUdata.C:233
unsigned short header2
Header 2 fields that indicate what data is valid.
Definition IMUdata.H:135
unsigned long stepts
Step detection timestamp (when JEVOIS_DMP_PED_STEPDET)
Definition IMUdata.H:146
long quat9[3]
Quaternion9 data (when JEVOIS_DMP_QUAT9 in header1)
Definition IMUdata.H:143
long geomagacc
Geomag accuracy (when JEVOIS_DMP_GEOMAG in header1)
Definition IMUdata.H:150
long quat6[3]
Quaternion6 data (when JEVOIS_DMP_QUAT6 in header1)
Definition IMUdata.H:142
float fsync_us() const
Delay between FSYNC and next IMU data, in microseconds.
Definition IMUdata.C:227
short pickup
Flip/pickup detection (when JEVOIS_DMP_FLIP_PICKUP in header2)
Definition IMUdata.H:162
unsigned short odrcnt
Output data rate counter (always here but unclear what it is)
Definition IMUdata.H:167
long geomag[3]
Geomag data (when JEVOIS_DMP_GEOMAG in header1)
Definition IMUdata.H:149
long bacts
Activity recognition timestamp (when JEVOIS_DMP_ACT_RECOG in header2)
Definition IMUdata.H:165
static float fix2float(long val)
Convert a long fixed-point value to float.
Definition IMUdata.C:283
short gbias[3]
Raw gyro bias data (when JEVOIS_DMP_GYRO in header1)
Definition IMUdata.H:140
IMU data.
Definition IMUdata.H:68
float & gy()
Definition IMUdata.H:83
float const & my() const
Definition IMUdata.H:92
float & gz()
Definition IMUdata.H:85
float const & temp() const
Definition IMUdata.H:88
float const & mz() const
Definition IMUdata.H:94
float const & mx() const
Definition IMUdata.H:90
float & gx()
Definition IMUdata.H:81
float & az()
Definition IMUdata.H:79
float & my()
Definition IMUdata.H:91
float const & ax() const
Definition IMUdata.H:76
float const & gx() const
Definition IMUdata.H:82
float const & ay() const
Definition IMUdata.H:78
float & mx()
Definition IMUdata.H:89
float & temp()
Definition IMUdata.H:87
float & mz()
Definition IMUdata.H:93
float v[10]
The values: ax, ay, az, gy, gy, gz, temp, mx, my, mz.
Definition IMUdata.H:72
float & ay()
Definition IMUdata.H:77
float const & gz() const
Definition IMUdata.H:86
float const & gy() const
Definition IMUdata.H:84
float const & az() const
Definition IMUdata.H:80
float & ax()
Definition IMUdata.H:75
bool magovf
True if magnetometer overflow.
Definition IMUdata.H:73
Raw IMU data.
Definition IMUdata.H:37
short v[11]
The values: ax, ay, az, gy, gy, gz, temp, mx, my, mz, mst2.
Definition IMUdata.H:38
short const & mst2() const
Definition IMUdata.H:61
short const & ax() const
Definition IMUdata.H:41
short const & my() const
Definition IMUdata.H:57
short & gy()
Definition IMUdata.H:48
short & az()
Definition IMUdata.H:44
short const & mz() const
Definition IMUdata.H:59
short const & temp() const
Definition IMUdata.H:53
short & mst2()
Definition IMUdata.H:60
short const & ay() const
Definition IMUdata.H:43
short const & gy() const
Definition IMUdata.H:49
short const & az() const
Definition IMUdata.H:45
short & mz()
Definition IMUdata.H:58
short & gx()
Definition IMUdata.H:46
short & my()
Definition IMUdata.H:56
short & temp()
Definition IMUdata.H:52
short & ay()
Definition IMUdata.H:42
short const & gx() const
Definition IMUdata.H:47
short const & gz() const
Definition IMUdata.H:51
short & gz()
Definition IMUdata.H:50
short const & mx() const
Definition IMUdata.H:55
short & ax()
Definition IMUdata.H:40
short & mx()
Definition IMUdata.H:54