JeVois  1.21
JeVois Smart Embedded Machine Vision Toolkit
Share this page:
Loading...
Searching...
No Matches
IMUspi.C
Go to the documentation of this file.
1// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2//
3// JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2020 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#ifdef JEVOIS_PRO
19
20#include <jevois/Core/IMUspi.H>
22#include <linux/spi/spidev.h>
23#include <jevois/Util/Utils.H>
24#include <jevois/Debug/Log.H>
25#include <thread>
26
27#include <sys/types.h>
28#include <sys/stat.h>
29#include <fcntl.h>
30#include <unistd.h>
31
32// ####################################################################################################
33jevois::IMUspi::IMUspi(std::string const & devname) :
34 jevois::IMU(), itsDevName(devname), itsFd(-1), itsIMUbank(0)
35{
36 // FIXME: clock speed seems to have no effect on whether we can load the DMP data or not.
37 // So setting retry to 1 for now, trying 7MHz only.
38 int retry = 1; // number of tries, halving the speed each time
39 unsigned int speed = 7000000;
40
41 while (retry)
42 {
43 try
44 {
45 itsFd = open(devname.c_str(), O_RDWR);
46 if (itsFd < 0) LFATAL("Error opening IMU SPI device " << devname);
47
48 unsigned char mode = SPI_CPOL | SPI_CPHA; // mode 3 for ICM20948
49 XIOCTL(itsFd, SPI_IOC_WR_MODE, &mode);
50
51 unsigned char bits = 8;
52 XIOCTL(itsFd, SPI_IOC_WR_BITS_PER_WORD, &bits);
53
54 XIOCTL(itsFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
55
56 // At the beginning, we need to read at least once to get the clock polarity in order:
57 readRegister(ICM20948_REG_WHO_AM_I);
58
59 // Force a reset. Reset bit will auto-clear:
60 writeRegister(ICM20948_REG_PWR_MGMT_1, ICM20948_BIT_H_RESET);
61
62 // Disable I2C as we use SPI, reset DMP:
63 writeRegister(ICM20948_REG_USER_CTRL, ICM20948_BIT_I2C_IF_DIS | ICM20948_BIT_DMP_RST |
64 ICM20948_BIT_DIAMOND_DMP_RST);
65
66 // Check that the ICM20948 is detected:
67 auto ret = readRegister(ICM20948_REG_WHO_AM_I);
68 if (ret == ICM20948_DEVICE_ID) LINFO("Detected ICM20948 IMU on " << devname << " @ " << speed << "Hz");
69 else LFATAL("Failed to detect ICM20948 on " << devname << " @ " << speed << "Hz: device ID=0x" << std::hex <<
70 ret << ", should be 0x" << ICM20948_DEVICE_ID);
71
72 // Init sequence for SPI operation:
73 writeRegister(ICM20948_REG_USER_CTRL, ICM20948_BIT_I2C_IF_DIS | ICM20948_BIT_I2C_MST_EN | ICM20948_BIT_FIFO_EN);
74 writeRegister(ICM20948_REG_PWR_MGMT_1, ICM20948_BIT_CLK_PLL);
75
76 // Upload the DMP firmware:
77 loadDMPfirmware(true, (retry > 1));
78
79 // Enable the DMP:
80 writeRegister(ICM20948_REG_USER_CTRL, ICM20948_BIT_I2C_IF_DIS | ICM20948_BIT_I2C_MST_EN | ICM20948_BIT_FIFO_EN |
81 ICM20948_BIT_DMP_EN);
82
83 // All good, let's get out of here:
84 return;
85 }
86 catch (...)
87 {
88 if (itsFd >= 0) close(itsFd);
89 --retry;
90 speed /= 2;
91 }
92 }
93 LERROR("Giving up trying to setup ICM20948 IMU with DMP -- DMP NOT OPERATIONAL, BASIC IMU MAY WORK OR NOT");
94}
95
96// ####################################################################################################
98{
99 if (itsFd >= 0) close(itsFd);
100}
101
102// ####################################################################################################
104{ return true; }
105
106// ####################################################################################################
107void jevois::IMUspi::spi_xfer(unsigned char reg, unsigned char dir, size_t siz, unsigned char * datain,
108 unsigned char const * dataout)
109{
110 unsigned char dreg = (reg & 0x7f) | dir;
111 unsigned int speed = 7000000;
112
113 struct spi_ioc_transfer xfer[2] =
114 {
115 {
116 .tx_buf = (unsigned long)&dreg,
117 .rx_buf = 0UL,
118 .len = 1,
119 .speed_hz = speed,
120 .delay_usecs = 0,
121 .bits_per_word = 8,
122 .cs_change = 0,
123 .tx_nbits = 8,
124 .rx_nbits = 8,
125 .word_delay_usecs = 0,
126 .pad = 0
127 },
128 {
129 .tx_buf = (unsigned long)dataout,
130 .rx_buf = (unsigned long)datain,
131 .len = static_cast<unsigned int>(siz),
132 .speed_hz = speed,
133 .delay_usecs = 0,
134 .bits_per_word = 8,
135 .cs_change = 0,
136 .tx_nbits = 8,
137 .rx_nbits = 8,
138 .word_delay_usecs = 0,
139 .pad = 0
140 }
141 };
142
143 XIOCTL(itsFd, SPI_IOC_MESSAGE(2), xfer);
144}
145
146// ####################################################################################################
147void jevois::IMUspi::selectBank(unsigned short reg)
148{
149 uint8_t const bank = (reg >> 7) & 0x03;
150 if (itsIMUbank == bank) return;
151
152 uint8_t dataout = uint8_t(bank << 4);
153 LDEBUG("Writing 0x" << std::hex << dataout << " to 0x" << ICM20948_REG_BANK_SEL);
154 spi_xfer(ICM20948_REG_BANK_SEL, ICM20948_SPI_WRITE, 1, nullptr, &dataout);
155 itsIMUbank = bank;
156}
157
158// ##############################################################################################################
159void jevois::IMUspi::writeRegister(unsigned short reg, unsigned char val)
160{
161 LDEBUG("Writing 0x" << std::hex << val << " to 0x" << reg);
162 selectBank(reg); unsigned char gogo;
163 spi_xfer(reg, ICM20948_SPI_WRITE, 1, &gogo/*nullptr*/, &val);
164
165 bool verify = true;
166 bool delay = false;
167
168 switch (reg)
169 {
170 // These registers always return 0, do not verify:
171 case ICM20948_REG_I2C_MST_CTRL:
172 case ICM20948_REG_I2C_SLV4_CTRL:
173 case ICM20948_REG_TEMP_CONFIG:
174 // These registers auto increment, do not verify:
175 case ICM20948_REG_MEM_START_ADDR:
176 case ICM20948_REG_MEM_R_W:
177 case ICM20948_REG_MEM_BANK_SEL:
178 // These registers have autoclear bits, do not verify:
179 case ICM20948_REG_USER_CTRL:
180 case ICM20948_REG_PWR_MGMT_1:
181 case ICM20948_REG_PWR_MGMT_2:
182 verify = false;
183 delay = true;
184 break;
185 }
186
187 if (delay) std::this_thread::sleep_for(std::chrono::milliseconds(5));
188
189 if (verify)
190 {
191 unsigned char ret = readRegister(reg);
192 if (ret != val)
193 {
194 LERROR("Read back reg 0x"<<std::hex<<reg<<" returned 0x"<<ret<<" instead of 0x"<<val);
195
196 // Try again:
197 spi_xfer(reg, ICM20948_SPI_WRITE, 1, nullptr, &val);
198
199 if (delay) std::this_thread::sleep_for(std::chrono::milliseconds(5));
200
201 ret = readRegister(reg);
202 if (ret != val)
203 LERROR("RETRY Read back reg 0x"<<std::hex<<reg<<" returned 0x"<<ret<<" instead of 0x"<<val);
204 else
205 LERROR("RETRY Read back reg 0x"<<std::hex<<reg<<" returned 0x"<<ret<<" -- OK");
206 }
207 }
208}
209
210// ##############################################################################################################
211unsigned char jevois::IMUspi::readRegister(unsigned short reg)
212{
213 selectBank(reg);
214 unsigned char datain; unsigned char gogo = 0;
215 spi_xfer(reg, ICM20948_SPI_READ, 1, &datain, &gogo/*nullptr*/);
216 LDEBUG("Register 0x" << std::hex << reg << " has value 0x" << datain);
217 return datain;
218}
219
220// ##############################################################################################################
221void jevois::IMUspi::writeRegisterArray(unsigned short reg, unsigned char const * vals, size_t num)
222{
223 if (num > 256) LFATAL("Maximum allowed size 256 bytes. You must break down larger transfers into 256 byte chunks.");
224 LDEBUG("Writing " << num << " values to 0x"<< std::hex << reg);
225 selectBank(reg);
226 spi_xfer(reg, ICM20948_SPI_WRITE, num, nullptr, vals);
227}
228
229// ##############################################################################################################
230void jevois::IMUspi::readRegisterArray(unsigned short reg, unsigned char * vals, size_t num)
231{
232 if (num > 256) LFATAL("Maximum allowed size 256 bytes. You must break down larger transfers into 256 byte chunks.");
233 selectBank(reg);
234 spi_xfer(reg, ICM20948_SPI_READ, num, vals, nullptr);
235 LDEBUG("Received " << num <<" values from register 0x" << std::hex << reg);
236}
237
238#endif // JEVOIS_PRO
#define ICM20948_SPI_WRITE
#define ICM20948_SPI_READ
Abstract interface to an ICM20948 inertial measurement unit (IMU)
Definition IMU.H:28
void loadDMPfirmware(bool verify=false, bool errthrow=false)
Load the DMP firmware.
Definition IMU.C:37
void writeRegisterArray(unsigned short reg, unsigned char const *vals, size_t num) override
Write an array of values to the camera's IMU registers.
Definition IMUspi.C:221
void writeRegister(unsigned short reg, unsigned char val) override
Write a value to one of the IMU registers.
Definition IMUspi.C:159
virtual ~IMUspi()
Destructor.
Definition IMUspi.C:97
virtual bool isSPI() const override
Returns true if we use SPI for transfers. Used when ICM20948_REG_USER_CTRL is written to.
Definition IMUspi.C:103
unsigned char readRegister(unsigned short reg) override
Read a value from one of the camera's IMU registers.
Definition IMUspi.C:211
void selectBank(unsigned short reg)
Definition IMUspi.C:147
IMUspi(std::string const &devname)
Constructor.
Definition IMUspi.C:33
void readRegisterArray(unsigned short reg, unsigned char *vals, size_t num) override
Read an array of values from the camera's IMU registers.
Definition IMUspi.C:230
void spi_xfer(unsigned char addr, unsigned char dir, size_t siz, unsigned char *datain, unsigned char const *dataout)
Definition IMUspi.C:107
#define LFATAL(msg)
Convenience macro for users to print out console or syslog messages, FATAL level.
Definition Log.H:230
#define LDEBUG(msg)
Convenience macro for users to print out console or syslog messages, DEBUG level.
Definition Log.H:173
#define LERROR(msg)
Convenience macro for users to print out console or syslog messages, ERROR level.
Definition Log.H:211
#define LINFO(msg)
Convenience macro for users to print out console or syslog messages, INFO level.
Definition Log.H:194
#define XIOCTL(dev, req, mem)
Helper macro to execute an ioctl, ignore interruptions, and, if error, issue a fatal message and thro...
Definition Utils.H:205
Main namespace for all JeVois classes and functions.
Definition Concepts.dox:2