JeVois  1.20
JeVois Smart Embedded Machine Vision Toolkit
Share this page:
IMU.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 #include <jevois/Core/IMU.H>
20 #include <jevois/Debug/Log.H>
21 #include <thread>
22 
23 // Icm20948 device requires a DMP image to be loaded on init
24 static unsigned char const dmp3_image[] = {
25 #include "ICM20948_dmp3a.H"
26 };
27 
28 // ####################################################################################################
30 { }
31 
32 // ####################################################################################################
34 { }
35 
36 // ####################################################################################################
37 void jevois::IMU::loadDMPfirmware(bool verify, bool errthrow)
38 {
39  unsigned char currbank = 0xff;
40 
41  LINFO("Loading ICM20948 DMP firmware...");
42 
43  unsigned short addr = DMP_LOAD_START; size_t chunksiz = DMP_MEM_BANK_SIZE - (addr % DMP_MEM_BANK_SIZE);
44  for (size_t i = 0; i < sizeof(dmp3_image); i += chunksiz)
45  {
46  // Select DMP memory bank if it changed (banks have DMP_MEM_BANK_SIZE bytes):
47  unsigned char const bank = addr / DMP_MEM_BANK_SIZE;
48  if (bank != currbank) { writeRegister(ICM20948_REG_MEM_BANK_SEL, bank); currbank = bank; }
49 
50  // Write load address:
51  writeRegister(ICM20948_REG_MEM_START_ADDR, addr & 0xff);
52 
53  LDEBUG(" Writing " << chunksiz << " bytes to MEMS addr 0x" << std::hex << addr);
54 
55  // Then write the data into REG_MEM_R_W:
56  if (addr + chunksiz > sizeof(dmp3_image)) chunksiz = sizeof(dmp3_image) - addr;
57  writeRegisterArray(ICM20948_REG_MEM_R_W, &dmp3_image[i], chunksiz);
58 
59  addr += chunksiz; chunksiz = DMP_MEM_BANK_SIZE;
60  }
61 
62  bool verify_error = false;
63 
64  if (verify)
65  {
66  LINFO("Verifying ICM20948 DMP firmware...");
67 
68  // Read the data back to verify:
69  addr = DMP_LOAD_START; currbank = 0xff; unsigned char buf[DMP_MEM_BANK_SIZE];
70  chunksiz = DMP_MEM_BANK_SIZE - (addr % DMP_MEM_BANK_SIZE);
71  for (size_t i = 0; i < sizeof(dmp3_image); i += chunksiz)
72  {
73  // Select DMP memory bank if it changed (banks have DMP_MEM_BANK_SIZE bytes):
74  unsigned char const bank = addr / DMP_MEM_BANK_SIZE;
75  if (bank != currbank) { writeRegister(ICM20948_REG_MEM_BANK_SEL, bank); currbank = bank; }
76 
77  // Write load address:
78  writeRegister(ICM20948_REG_MEM_START_ADDR, addr & 0xff);
79 
80  LDEBUG(" Reading " << chunksiz << " bytes from MEMS addr 0x" << std::hex << addr);
81 
82  // Then read the data from REG_MEM_R_W:
83  if (addr + chunksiz > sizeof(dmp3_image)) chunksiz = sizeof(dmp3_image) - addr;
84  readRegisterArray(ICM20948_REG_MEM_R_W, buf, chunksiz);
85  for (size_t j = 0; j < chunksiz; ++j)
86  if (buf[j] != dmp3_image[i + j])
87  {
88  LDEBUG("DMP code verify error addr=" << std::hex << std::showbase << addr + j << ", read=" << buf[j] <<
89  ", orig=" << dmp3_image[i + j]);
90  verify_error = true;
91  }
92  addr += chunksiz; chunksiz = DMP_MEM_BANK_SIZE;
93  }
94  }
95 
96  // Get back to MEMS bank 0:
97  writeRegister(ICM20948_REG_MEM_BANK_SEL, 0);
98 
99  // Set the DMP start address:
100  unsigned char dmp_addr[2] = { (DMP_START_ADDRESS >> 8) & 0xff, DMP_START_ADDRESS & 0xff };
101  writeRegisterArray(ICM20948_REG_PRGM_START_ADDRH, &dmp_addr[0], 2);
102 
103  LINFO("Loaded " << sizeof(dmp3_image) << " bytes of DMP firmware.");
104  // User code will actually enable the DMP if desired.
105 
106  if (verify_error)
107  {
108  if (errthrow) LFATAL("Error during DMP firmware verify -- DMP NOT OPERATIONAL");
109  else LERROR("Error during DMP firmware verify -- DMP NOT OPERATIONAL");
110  }
111 }
112 
113 // ####################################################################################################
114 void jevois::IMU::writeDMPregister(unsigned short reg, unsigned short val)
115 {
116  // Write the data in big endian:
117  unsigned char data[2];
118  data[0] = val >> 8;
119  data[1] = val & 0xff;
120 
121  writeDMPregisterArray(reg, &data[0], 2);
122 }
123 
124 // ####################################################################################################
125 void jevois::IMU::writeDMPregisterArray(unsigned short reg, unsigned char const * vals, size_t num)
126 {
127  // Select MEMs bank from the 8 MSBs of reg:
128  writeRegister(ICM20948_REG_MEM_BANK_SEL, reg >> 8);
129 
130  // Set address:
131  writeRegister(ICM20948_REG_MEM_START_ADDR, reg & 0xff);
132 
133  // Write data:
134  writeRegisterArray(ICM20948_REG_MEM_R_W, vals, num);
135 }
136 
137 // ####################################################################################################
138 unsigned short jevois::IMU::readDMPregister(unsigned short reg)
139 {
140  // Read data in big endian:
141  unsigned char data[2];
142  readDMPregisterArray(reg, &data[0], 2);
143 
144  // Return it in little endian:
145  return (data[0] << 8) | data[1];
146 }
147 
148 // ####################################################################################################
149 void jevois::IMU::readDMPregisterArray(unsigned short reg, unsigned char * vals, size_t num)
150 {
151  // Select MEMs bank from the 8 MSBs of reg:
152  writeRegister(ICM20948_REG_MEM_BANK_SEL, reg >> 8);
153 
154  // Set address:
155  writeRegister(ICM20948_REG_MEM_START_ADDR, reg & 0xff);
156 
157  // Write data:
158  readRegisterArray(ICM20948_REG_MEM_R_W, vals, num);
159 }
jevois::IMU::loadDMPfirmware
void loadDMPfirmware(bool verify=false, bool errthrow=false)
Load the DMP firmware.
Definition: IMU.C:37
LDEBUG
#define LDEBUG(msg)
Convenience macro for users to print out console or syslog messages, DEBUG level.
Definition: Log.H:173
jevois::IMU::~IMU
virtual ~IMU()
Destructor.
Definition: IMU.C:33
DMP_LOAD_START
#define DMP_LOAD_START
Definition: ICM20948_regs.H:643
jevois::IMU::IMU
IMU()
Constructor.
Definition: IMU.C:29
jevois::IMU::readDMPregister
unsigned short readDMPregister(unsigned short reg)
Read a value from a DMP register.
Definition: IMU.C:138
ICM20948_dmp3a.H
LERROR
#define LERROR(msg)
Convenience macro for users to print out console or syslog messages, ERROR level.
Definition: Log.H:211
Log.H
ICM20948_regs.H
jevois::IMU::writeDMPregisterArray
void writeDMPregisterArray(unsigned short reg, unsigned char const *vals, size_t num)
Write an array of values to DMP registers.
Definition: IMU.C:125
LFATAL
#define LFATAL(msg)
Convenience macro for users to print out console or syslog messages, FATAL level.
jevois::IMU::writeDMPregister
void writeDMPregister(unsigned short reg, unsigned short val)
Write a value to a DMP register.
Definition: IMU.C:114
jevois::IMU::readDMPregisterArray
void readDMPregisterArray(unsigned short reg, unsigned char *vals, size_t num)
Read an array of values from DMP registers.
Definition: IMU.C:149
DMP_START_ADDRESS
#define DMP_START_ADDRESS
Definition: ICM20948_regs.H:641
LINFO
#define LINFO(msg)
Convenience macro for users to print out console or syslog messages, INFO level.
Definition: Log.H:194
IMU.H
DMP_MEM_BANK_SIZE
#define DMP_MEM_BANK_SIZE
Definition: ICM20948_regs.H:642