JeVois  1.17
JeVois Smart Embedded Machine Vision Toolkit
Share this page:
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 // ####################################################################################################
33 jevois::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 // ####################################################################################################
107 void 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 // ####################################################################################################
147 void 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 // ##############################################################################################################
159 void 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 // ##############################################################################################################
211 unsigned 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 // ##############################################################################################################
221 void 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 // ##############################################################################################################
230 void 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
jevois::IMUspi::itsFd
int itsFd
Definition: IMUspi.H:63
jevois::IMU::loadDMPfirmware
void loadDMPfirmware(bool verify=false, bool errthrow=false)
Load the DMP firmware.
Definition: IMU.C:37
jevois::imu::mode
Magnetometer sampling or Off to disable or Once to only get one measurement You can repeatedly set this parameter to Once to obtain repeated measurements at your own pace In JeVois you need to alternate between Off and Once In FIFO mode
Definition: ICM20948.H:115
LDEBUG
#define LDEBUG(msg)
Convenience macro for users to print out console or syslog messages, DEBUG level.
Definition: Log.H:160
jevois::IMUspi::writeRegisterArray
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
jevois::IMUspi::isSPI
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
jevois::IMUspi::writeRegister
void writeRegister(unsigned short reg, unsigned char val) override
Write a value to one of the IMU registers.
Definition: IMUspi.C:159
LERROR
#define LERROR(msg)
Convenience macro for users to print out console or syslog messages, ERROR level.
Definition: Log.H:198
ICM20948_SPI_WRITE
#define ICM20948_SPI_WRITE
Definition: ICM20948_regs.H:20
jevois::IMUspi::~IMUspi
virtual ~IMUspi()
Destructor.
Definition: IMUspi.C:97
jevois
Definition: Concepts.dox:1
Log.H
jevois::IMUspi::IMUspi
IMUspi(std::string const &devname)
Constructor.
Definition: IMUspi.C:33
ICM20948_regs.H
jevois::IMU
Abstract interface to an ICM20948 inertial measurement unit (IMU)
Definition: IMU.H:27
LFATAL
#define LFATAL(msg)
Convenience macro for users to print out console or syslog messages, FATAL level.
Definition: Log.H:217
ICM20948_SPI_READ
#define ICM20948_SPI_READ
Definition: ICM20948_regs.H:19
jevois::IMUspi::readRegister
unsigned char readRegister(unsigned short reg) override
Read a value from one of the camera's IMU registers.
Definition: IMUspi.C:211
Utils.H
IMUspi.H
LINFO
#define LINFO(msg)
Convenience macro for users to print out console or syslog messages, INFO level.
Definition: Log.H:181
jevois::IMUspi::spi_xfer
void spi_xfer(unsigned char addr, unsigned char dir, size_t siz, unsigned char *datain, unsigned char const *dataout)
Definition: IMUspi.C:107
jevois::IMUspi::selectBank
void selectBank(unsigned short reg)
Definition: IMUspi.C:147
jevois::IMUspi::readRegisterArray
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