1991 lines
68 KiB
C++
1991 lines
68 KiB
C++
#include "ICM_20948.h"
|
||
|
||
#include "ICM_20948_REGISTERS.h"
|
||
#include "AK09916_REGISTERS.h"
|
||
|
||
// Forward Declarations
|
||
ICM_20948_Status_e ICM_20948_write_I2C(uint8_t reg, uint8_t *data, uint32_t len, void *user);
|
||
ICM_20948_Status_e ICM_20948_read_I2C(uint8_t reg, uint8_t *buff, uint32_t len, void *user);
|
||
ICM_20948_Status_e ICM_20948_write_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user);
|
||
ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user);
|
||
|
||
// Base
|
||
ICM_20948::ICM_20948()
|
||
{
|
||
status = ICM_20948_init_struct(&_device);
|
||
}
|
||
|
||
void ICM_20948::enableDebugging(Stream &debugPort)
|
||
{
|
||
_debugSerial = &debugPort; //Grab which port the user wants us to use for debugging
|
||
_printDebug = true; //Should we print the commands we send? Good for debugging
|
||
}
|
||
void ICM_20948::disableDebugging(void)
|
||
{
|
||
_printDebug = false; //Turn off extra print statements
|
||
}
|
||
|
||
// Debug Printing: based on gfvalvo's flash string helper code:
|
||
// https://forum.arduino.cc/index.php?topic=533118.msg3634809#msg3634809
|
||
|
||
void ICM_20948::debugPrint(const char *line)
|
||
{
|
||
doDebugPrint([](const char *ptr) { return *ptr; }, line);
|
||
}
|
||
|
||
void ICM_20948::debugPrint(const __FlashStringHelper *line)
|
||
{
|
||
doDebugPrint([](const char *ptr) { return (char)pgm_read_byte_near(ptr); },
|
||
(const char *)line);
|
||
}
|
||
|
||
void ICM_20948::debugPrintln(const char *line)
|
||
{
|
||
doDebugPrint([](const char *ptr) { return *ptr; }, line, true);
|
||
}
|
||
|
||
void ICM_20948::debugPrintln(const __FlashStringHelper *line)
|
||
{
|
||
doDebugPrint([](const char *ptr) { return (char)pgm_read_byte_near(ptr); },
|
||
(const char *)line, true);
|
||
}
|
||
|
||
void ICM_20948::doDebugPrint(char (*funct)(const char *), const char *string, bool newLine)
|
||
{
|
||
if (_printDebug == false)
|
||
return; // Bail if debugging is not enabled
|
||
|
||
char ch;
|
||
|
||
while ((ch = funct(string++)))
|
||
{
|
||
_debugSerial->print(ch);
|
||
}
|
||
|
||
if (newLine)
|
||
{
|
||
_debugSerial->println();
|
||
}
|
||
}
|
||
|
||
void ICM_20948::debugPrintf(int i)
|
||
{
|
||
if (_printDebug == true)
|
||
_debugSerial->print(i);
|
||
}
|
||
|
||
void ICM_20948::debugPrintf(float f)
|
||
{
|
||
if (_printDebug == true)
|
||
_debugSerial->print(f);
|
||
}
|
||
|
||
void ICM_20948::debugPrintStatus(ICM_20948_Status_e stat)
|
||
{
|
||
switch (stat)
|
||
{
|
||
case ICM_20948_Stat_Ok:
|
||
debugPrint(F("All is well."));
|
||
break;
|
||
case ICM_20948_Stat_Err:
|
||
debugPrint(F("General Error"));
|
||
break;
|
||
case ICM_20948_Stat_NotImpl:
|
||
debugPrint(F("Not Implemented"));
|
||
break;
|
||
case ICM_20948_Stat_ParamErr:
|
||
debugPrint(F("Parameter Error"));
|
||
break;
|
||
case ICM_20948_Stat_WrongID:
|
||
debugPrint(F("Wrong ID"));
|
||
break;
|
||
case ICM_20948_Stat_InvalSensor:
|
||
debugPrint(F("Invalid Sensor"));
|
||
break;
|
||
case ICM_20948_Stat_NoData:
|
||
debugPrint(F("Data Underflow"));
|
||
break;
|
||
case ICM_20948_Stat_SensorNotSupported:
|
||
debugPrint(F("Sensor Not Supported"));
|
||
break;
|
||
case ICM_20948_Stat_DMPNotSupported:
|
||
debugPrint(F("DMP Firmware Not Supported. Is #define ICM_20948_USE_DMP commented in util/ICM_20948_C.h?"));
|
||
break;
|
||
case ICM_20948_Stat_DMPVerifyFail:
|
||
debugPrint(F("DMP Firmware Verification Failed"));
|
||
break;
|
||
case ICM_20948_Stat_FIFONoDataAvail:
|
||
debugPrint(F("No FIFO Data Available"));
|
||
break;
|
||
case ICM_20948_Stat_FIFOIncompleteData:
|
||
debugPrint(F("DMP data in FIFO was incomplete"));
|
||
break;
|
||
case ICM_20948_Stat_FIFOMoreDataAvail:
|
||
debugPrint(F("More FIFO Data Available"));
|
||
break;
|
||
case ICM_20948_Stat_UnrecognisedDMPHeader:
|
||
debugPrint(F("Unrecognised DMP Header"));
|
||
break;
|
||
case ICM_20948_Stat_UnrecognisedDMPHeader2:
|
||
debugPrint(F("Unrecognised DMP Header2"));
|
||
break;
|
||
case ICM_20948_Stat_InvalDMPRegister:
|
||
debugPrint(F("Invalid DMP Register"));
|
||
break;
|
||
default:
|
||
debugPrint(F("Unknown Status"));
|
||
break;
|
||
}
|
||
}
|
||
|
||
ICM_20948_AGMT_t ICM_20948::getAGMT(void)
|
||
{
|
||
status = ICM_20948_get_agmt(&_device, &agmt);
|
||
|
||
return agmt;
|
||
}
|
||
|
||
float ICM_20948::magX(void)
|
||
{
|
||
return getMagUT(agmt.mag.axes.x);
|
||
}
|
||
|
||
float ICM_20948::magY(void)
|
||
{
|
||
return getMagUT(agmt.mag.axes.y);
|
||
}
|
||
|
||
float ICM_20948::magZ(void)
|
||
{
|
||
return getMagUT(agmt.mag.axes.z);
|
||
}
|
||
|
||
float ICM_20948::getMagUT(int16_t axis_val)
|
||
{
|
||
return (((float)axis_val) * 0.15);
|
||
}
|
||
|
||
float ICM_20948::accX(void)
|
||
{
|
||
return getAccMG(agmt.acc.axes.x);
|
||
}
|
||
|
||
float ICM_20948::accY(void)
|
||
{
|
||
return getAccMG(agmt.acc.axes.y);
|
||
}
|
||
|
||
float ICM_20948::accZ(void)
|
||
{
|
||
return getAccMG(agmt.acc.axes.z);
|
||
}
|
||
|
||
float ICM_20948::getAccMG(int16_t axis_val)
|
||
{
|
||
switch (agmt.fss.a)
|
||
{
|
||
case 0:
|
||
return (((float)axis_val) / 16.384);
|
||
break;
|
||
case 1:
|
||
return (((float)axis_val) / 8.192);
|
||
break;
|
||
case 2:
|
||
return (((float)axis_val) / 4.096);
|
||
break;
|
||
case 3:
|
||
return (((float)axis_val) / 2.048);
|
||
break;
|
||
default:
|
||
return 0;
|
||
break;
|
||
}
|
||
}
|
||
|
||
float ICM_20948::gyrX(void)
|
||
{
|
||
return getGyrDPS(agmt.gyr.axes.x);
|
||
}
|
||
|
||
float ICM_20948::gyrY(void)
|
||
{
|
||
return getGyrDPS(agmt.gyr.axes.y);
|
||
}
|
||
|
||
float ICM_20948::gyrZ(void)
|
||
{
|
||
return getGyrDPS(agmt.gyr.axes.z);
|
||
}
|
||
|
||
float ICM_20948::getGyrDPS(int16_t axis_val)
|
||
{
|
||
switch (agmt.fss.g)
|
||
{
|
||
case 0:
|
||
return (((float)axis_val) / 131);
|
||
break;
|
||
case 1:
|
||
return (((float)axis_val) / 65.5);
|
||
break;
|
||
case 2:
|
||
return (((float)axis_val) / 32.8);
|
||
break;
|
||
case 3:
|
||
return (((float)axis_val) / 16.4);
|
||
break;
|
||
default:
|
||
return 0;
|
||
break;
|
||
}
|
||
}
|
||
|
||
//Gyro Bias
|
||
ICM_20948_Status_e ICM_20948::SetBiasGyroX( int newValue)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char gyro_bias_reg[4];
|
||
gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
|
||
gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
|
||
gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
|
||
gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
|
||
result = inv_icm20948_write_mems(&_device, GYRO_BIAS_X, 4, (const unsigned char*)&gyro_bias_reg);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::SetBiasGyroY( int newValue)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char gyro_bias_reg[4];
|
||
gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
|
||
gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
|
||
gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
|
||
gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
|
||
result = inv_icm20948_write_mems(&_device, GYRO_BIAS_Y, 4, (const unsigned char*)&gyro_bias_reg);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::SetBiasGyroZ( int newValue)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char gyro_bias_reg[4];
|
||
gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
|
||
gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
|
||
gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
|
||
gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
|
||
result = inv_icm20948_write_mems(&_device, GYRO_BIAS_Z, 4, (const unsigned char*)&gyro_bias_reg);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::GetBiasGyroX( int* bias)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char bias_data[4] = { 0 };
|
||
result = inv_icm20948_read_mems(&_device, GYRO_BIAS_X, 4, bias_data);
|
||
bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::GetBiasGyroY( int* bias)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char bias_data[4] = { 0 };
|
||
result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Y, 4, bias_data);
|
||
bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::GetBiasGyroZ( int* bias)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char bias_data[4] = { 0 };
|
||
result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Z, 4, bias_data);
|
||
bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
|
||
return result;
|
||
}
|
||
//Accel Bias
|
||
ICM_20948_Status_e ICM_20948::SetBiasAccelX( int newValue)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char accel_bias_reg[4];
|
||
accel_bias_reg[0] = (unsigned char)(newValue >> 24);
|
||
accel_bias_reg[1] = (unsigned char)(newValue >> 16);
|
||
accel_bias_reg[2] = (unsigned char)(newValue >> 8);
|
||
accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
|
||
result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_X, 4, (const unsigned char*)&accel_bias_reg);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::SetBiasAccelY( int newValue)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char accel_bias_reg[4];
|
||
accel_bias_reg[0] = (unsigned char)(newValue >> 24);
|
||
accel_bias_reg[1] = (unsigned char)(newValue >> 16);
|
||
accel_bias_reg[2] = (unsigned char)(newValue >> 8);
|
||
accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
|
||
result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Y, 4, (const unsigned char*)&accel_bias_reg);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::SetBiasAccelZ( int newValue)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char accel_bias_reg[4];
|
||
accel_bias_reg[0] = (unsigned char)(newValue >> 24);
|
||
accel_bias_reg[1] = (unsigned char)(newValue >> 16);
|
||
accel_bias_reg[2] = (unsigned char)(newValue >> 8);
|
||
accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
|
||
result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Z, 4, (const unsigned char*)&accel_bias_reg);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::GetBiasAccelX( int* bias)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char bias_data[4] = { 0 };
|
||
result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_X, 4, bias_data);
|
||
bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::GetBiasAccelY( int* bias)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char bias_data[4] = { 0 };
|
||
result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Y, 4, bias_data);
|
||
bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::GetBiasAccelZ( int* bias)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char bias_data[4] = { 0 };
|
||
result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Z, 4, bias_data);
|
||
bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
|
||
return result;
|
||
}
|
||
//CPass Bias
|
||
ICM_20948_Status_e ICM_20948::SetBiasCPassX( int newValue)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char cpass_bias_reg[4];
|
||
cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
|
||
cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
|
||
cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
|
||
cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
|
||
result = inv_icm20948_write_mems(&_device, CPASS_BIAS_X, 4, (const unsigned char*)&cpass_bias_reg);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::SetBiasCPassY( int newValue)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char cpass_bias_reg[4];
|
||
cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
|
||
cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
|
||
cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
|
||
cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
|
||
result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Y, 4, (const unsigned char*)&cpass_bias_reg);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::SetBiasCPassZ( int newValue)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char cpass_bias_reg[4];
|
||
cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
|
||
cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
|
||
cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
|
||
cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
|
||
result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Z, 4, (const unsigned char*)&cpass_bias_reg);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::GetBiasCPassX( int* bias)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char bias_data[4] = { 0 };
|
||
result = inv_icm20948_read_mems(&_device, CPASS_BIAS_X, 4, bias_data);
|
||
bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::GetBiasCPassY( int* bias)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char bias_data[4] = { 0 };
|
||
result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Y, 4, bias_data);
|
||
bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
|
||
return result;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::GetBiasCPassZ( int* bias)
|
||
{
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
|
||
unsigned char bias_data[4] = { 0 };
|
||
result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Z, 4, bias_data);
|
||
bias[0] = (int)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
|
||
return result;
|
||
}
|
||
|
||
float ICM_20948::temp(void)
|
||
{
|
||
return getTempC(agmt.tmp.val);
|
||
}
|
||
|
||
float ICM_20948::getTempC(int16_t val)
|
||
{
|
||
return (((float)val - 21) / 333.87) + 21;
|
||
}
|
||
|
||
const char *ICM_20948::statusString(ICM_20948_Status_e stat)
|
||
{
|
||
ICM_20948_Status_e val;
|
||
if (stat == ICM_20948_Stat_NUM)
|
||
{
|
||
val = status;
|
||
}
|
||
else
|
||
{
|
||
val = stat;
|
||
}
|
||
|
||
switch (val)
|
||
{
|
||
case ICM_20948_Stat_Ok:
|
||
return "All is well.";
|
||
break;
|
||
case ICM_20948_Stat_Err:
|
||
return "General Error";
|
||
break;
|
||
case ICM_20948_Stat_NotImpl:
|
||
return "Not Implemented";
|
||
break;
|
||
case ICM_20948_Stat_ParamErr:
|
||
return "Parameter Error";
|
||
break;
|
||
case ICM_20948_Stat_WrongID:
|
||
return "Wrong ID";
|
||
break;
|
||
case ICM_20948_Stat_InvalSensor:
|
||
return "Invalid Sensor";
|
||
break;
|
||
case ICM_20948_Stat_NoData:
|
||
return "Data Underflow";
|
||
break;
|
||
case ICM_20948_Stat_SensorNotSupported:
|
||
return "Sensor Not Supported";
|
||
break;
|
||
case ICM_20948_Stat_DMPNotSupported:
|
||
return "DMP Firmware Not Supported. Is #define ICM_20948_USE_DMP commented in util/ICM_20948_C.h?";
|
||
break;
|
||
case ICM_20948_Stat_DMPVerifyFail:
|
||
return "DMP Firmware Verification Failed";
|
||
break;
|
||
case ICM_20948_Stat_FIFONoDataAvail:
|
||
return "No FIFO Data Available";
|
||
break;
|
||
case ICM_20948_Stat_FIFOIncompleteData:
|
||
return "DMP data in FIFO was incomplete";
|
||
break;
|
||
case ICM_20948_Stat_FIFOMoreDataAvail:
|
||
return "More FIFO Data Available";
|
||
break;
|
||
case ICM_20948_Stat_UnrecognisedDMPHeader:
|
||
return "Unrecognised DMP Header";
|
||
break;
|
||
case ICM_20948_Stat_UnrecognisedDMPHeader2:
|
||
return "Unrecognised DMP Header2";
|
||
break;
|
||
case ICM_20948_Stat_InvalDMPRegister:
|
||
return "Invalid DMP Register";
|
||
break;
|
||
default:
|
||
return "Unknown Status";
|
||
break;
|
||
}
|
||
return "None";
|
||
}
|
||
|
||
// Device Level
|
||
ICM_20948_Status_e ICM_20948::setBank(uint8_t bank)
|
||
{
|
||
status = ICM_20948_set_bank(&_device, bank);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::swReset(void)
|
||
{
|
||
status = ICM_20948_sw_reset(&_device);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::sleep(bool on)
|
||
{
|
||
status = ICM_20948_sleep(&_device, on);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::lowPower(bool on)
|
||
{
|
||
status = ICM_20948_low_power(&_device, on);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::setClockSource(ICM_20948_PWR_MGMT_1_CLKSEL_e source)
|
||
{
|
||
status = ICM_20948_set_clock_source(&_device, source);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::checkID(void)
|
||
{
|
||
status = ICM_20948_check_id(&_device);
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::checkID: ICM_20948_check_id returned: "));
|
||
debugPrintStatus(status);
|
||
debugPrintln(F(""));
|
||
}
|
||
return status;
|
||
}
|
||
|
||
bool ICM_20948::dataReady(void)
|
||
{
|
||
status = ICM_20948_data_ready(&_device);
|
||
if (status == ICM_20948_Stat_Ok)
|
||
{
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
uint8_t ICM_20948::getWhoAmI(void)
|
||
{
|
||
uint8_t retval = 0x00;
|
||
status = ICM_20948_get_who_am_i(&_device, &retval);
|
||
return retval;
|
||
}
|
||
|
||
bool ICM_20948::isConnected(void)
|
||
{
|
||
status = checkID();
|
||
if (status == ICM_20948_Stat_Ok)
|
||
{
|
||
return true;
|
||
}
|
||
debugPrint(F("ICM_20948::isConnected: checkID returned: "));
|
||
debugPrintStatus(status);
|
||
debugPrintln(F(""));
|
||
return false;
|
||
}
|
||
|
||
// Internal Sensor Options
|
||
ICM_20948_Status_e ICM_20948::setSampleMode(uint8_t sensor_id_bm, uint8_t lp_config_cycle_mode)
|
||
{
|
||
status = ICM_20948_set_sample_mode(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, (ICM_20948_LP_CONFIG_CYCLE_e)lp_config_cycle_mode);
|
||
delay(1); // Give the ICM20948 time to change the sample mode (see issue #8)
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::setFullScale(uint8_t sensor_id_bm, ICM_20948_fss_t fss)
|
||
{
|
||
status = ICM_20948_set_full_scale(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, fss);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::setDLPFcfg(uint8_t sensor_id_bm, ICM_20948_dlpcfg_t cfg)
|
||
{
|
||
status = ICM_20948_set_dlpf_cfg(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, cfg);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::enableDLPF(uint8_t sensor_id_bm, bool enable)
|
||
{
|
||
status = ICM_20948_enable_dlpf(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, enable);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::setSampleRate(uint8_t sensor_id_bm, ICM_20948_smplrt_t smplrt)
|
||
{
|
||
status = ICM_20948_set_sample_rate(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, smplrt);
|
||
return status;
|
||
}
|
||
|
||
// Interrupts on INT Pin
|
||
ICM_20948_Status_e ICM_20948::clearInterrupts(void)
|
||
{
|
||
ICM_20948_INT_STATUS_t int_stat;
|
||
ICM_20948_INT_STATUS_1_t int_stat_1;
|
||
|
||
// read to clear interrupts
|
||
status = ICM_20948_set_bank(&_device, 0);
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
status = ICM_20948_execute_r(&_device, AGB0_REG_INT_STATUS, (uint8_t *)&int_stat, sizeof(ICM_20948_INT_STATUS_t));
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
status = ICM_20948_execute_r(&_device, AGB0_REG_INT_STATUS_1, (uint8_t *)&int_stat_1, sizeof(ICM_20948_INT_STATUS_1_t));
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
|
||
// todo: there may be additional interrupts that need to be cleared, like FIFO overflow/watermark
|
||
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::cfgIntActiveLow(bool active_low)
|
||
{
|
||
ICM_20948_INT_PIN_CFG_t reg;
|
||
status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
reg.INT1_ACTL = active_low; // set the setting
|
||
status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::cfgIntOpenDrain(bool open_drain)
|
||
{
|
||
ICM_20948_INT_PIN_CFG_t reg;
|
||
status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
reg.INT1_OPEN = open_drain; // set the setting
|
||
status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::cfgIntLatch(bool latching)
|
||
{
|
||
ICM_20948_INT_PIN_CFG_t reg;
|
||
status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
reg.INT1_LATCH_EN = latching; // set the setting
|
||
status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::cfgIntAnyReadToClear(bool enabled)
|
||
{
|
||
ICM_20948_INT_PIN_CFG_t reg;
|
||
status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
reg.INT_ANYRD_2CLEAR = enabled; // set the setting
|
||
status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::cfgFsyncActiveLow(bool active_low)
|
||
{
|
||
ICM_20948_INT_PIN_CFG_t reg;
|
||
status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
reg.ACTL_FSYNC = active_low; // set the setting
|
||
status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::cfgFsyncIntMode(bool interrupt_mode)
|
||
{
|
||
ICM_20948_INT_PIN_CFG_t reg;
|
||
status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
reg.FSYNC_INT_MODE_EN = interrupt_mode; // set the setting
|
||
status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
// All these individual functions will use a read->set->write method to leave other settings untouched
|
||
ICM_20948_Status_e ICM_20948::intEnableI2C(bool enable)
|
||
{
|
||
ICM_20948_INT_enable_t en; // storage
|
||
status = ICM_20948_int_enable(&_device, NULL, &en); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
en.I2C_MST_INT_EN = enable; // change the setting
|
||
status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
if (en.I2C_MST_INT_EN != enable)
|
||
{
|
||
status = ICM_20948_Stat_Err;
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::intEnableDMP(bool enable)
|
||
{
|
||
ICM_20948_INT_enable_t en; // storage
|
||
status = ICM_20948_int_enable(&_device, NULL, &en); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
en.DMP_INT1_EN = enable; // change the setting
|
||
status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
if (en.DMP_INT1_EN != enable)
|
||
{
|
||
status = ICM_20948_Stat_Err;
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::intEnablePLL(bool enable)
|
||
{
|
||
ICM_20948_INT_enable_t en; // storage
|
||
status = ICM_20948_int_enable(&_device, NULL, &en); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
en.PLL_RDY_EN = enable; // change the setting
|
||
status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
if (en.PLL_RDY_EN != enable)
|
||
{
|
||
status = ICM_20948_Stat_Err;
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::intEnableWOM(bool enable)
|
||
{
|
||
ICM_20948_INT_enable_t en; // storage
|
||
status = ICM_20948_int_enable(&_device, NULL, &en); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
en.WOM_INT_EN = enable; // change the setting
|
||
status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
if (en.WOM_INT_EN != enable)
|
||
{
|
||
status = ICM_20948_Stat_Err;
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::intEnableWOF(bool enable)
|
||
{
|
||
ICM_20948_INT_enable_t en; // storage
|
||
status = ICM_20948_int_enable(&_device, NULL, &en); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
en.REG_WOF_EN = enable; // change the setting
|
||
status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
if (en.REG_WOF_EN != enable)
|
||
{
|
||
status = ICM_20948_Stat_Err;
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::intEnableRawDataReady(bool enable)
|
||
{
|
||
ICM_20948_INT_enable_t en; // storage
|
||
status = ICM_20948_int_enable(&_device, NULL, &en); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
en.RAW_DATA_0_RDY_EN = enable; // change the setting
|
||
status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
if (en.RAW_DATA_0_RDY_EN != enable)
|
||
{
|
||
status = ICM_20948_Stat_Err;
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::intEnableOverflowFIFO(uint8_t bm_enable)
|
||
{
|
||
ICM_20948_INT_enable_t en; // storage
|
||
status = ICM_20948_int_enable(&_device, NULL, &en); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
en.FIFO_OVERFLOW_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings
|
||
en.FIFO_OVERFLOW_EN_1 = ((bm_enable >> 1) & 0x01);
|
||
en.FIFO_OVERFLOW_EN_2 = ((bm_enable >> 2) & 0x01);
|
||
en.FIFO_OVERFLOW_EN_3 = ((bm_enable >> 3) & 0x01);
|
||
en.FIFO_OVERFLOW_EN_4 = ((bm_enable >> 4) & 0x01);
|
||
status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::intEnableWatermarkFIFO(uint8_t bm_enable)
|
||
{
|
||
ICM_20948_INT_enable_t en; // storage
|
||
status = ICM_20948_int_enable(&_device, NULL, &en); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
en.FIFO_WM_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings
|
||
en.FIFO_WM_EN_1 = ((bm_enable >> 1) & 0x01);
|
||
en.FIFO_WM_EN_2 = ((bm_enable >> 2) & 0x01);
|
||
en.FIFO_WM_EN_3 = ((bm_enable >> 3) & 0x01);
|
||
en.FIFO_WM_EN_4 = ((bm_enable >> 4) & 0x01);
|
||
status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::WOMThreshold(uint8_t threshold)
|
||
{
|
||
ICM_20948_ACCEL_WOM_THR_t thr; // storage
|
||
status = ICM_20948_wom_threshold(&_device, NULL, &thr); // read phase
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
thr.WOM_THRESHOLD = threshold; // change the setting
|
||
status = ICM_20948_wom_threshold(&_device, &thr, &thr); // write phase w/ readback
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
return status;
|
||
}
|
||
if (thr.WOM_THRESHOLD != threshold)
|
||
{
|
||
status = ICM_20948_Stat_Err;
|
||
return status;
|
||
}
|
||
return status;
|
||
}
|
||
|
||
// Interface Options
|
||
ICM_20948_Status_e ICM_20948::i2cMasterPassthrough(bool passthrough)
|
||
{
|
||
status = ICM_20948_i2c_master_passthrough(&_device, passthrough);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::i2cMasterEnable(bool enable)
|
||
{
|
||
status = ICM_20948_i2c_master_enable(&_device, enable);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::i2cMasterReset()
|
||
{
|
||
status = ICM_20948_i2c_master_reset(&_device);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::i2cControllerConfigurePeripheral(uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap, uint8_t dataOut)
|
||
{
|
||
status = ICM_20948_i2c_controller_configure_peripheral(&_device, peripheral, addr, reg, len, Rw, enable, data_only, grp, swap, dataOut);
|
||
return status;
|
||
}
|
||
|
||
//Provided for backward-compatibility only. Please update to i2cControllerConfigurePeripheral and i2cControllerPeriph4Transaction.
|
||
//https://www.oshwa.org/2020/06/29/a-resolution-to-redefine-spi-pin-names/
|
||
ICM_20948_Status_e ICM_20948::i2cMasterConfigureSlave(uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap)
|
||
{
|
||
return (i2cControllerConfigurePeripheral(peripheral, addr, reg, len, Rw, enable, data_only, grp, swap, 0));
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::i2cControllerPeriph4Transaction(uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr)
|
||
{
|
||
status = ICM_20948_i2c_controller_periph4_txn(&_device, addr, reg, data, len, Rw, send_reg_addr);
|
||
return status;
|
||
}
|
||
|
||
//Provided for backward-compatibility only. Please update to i2cControllerConfigurePeripheral and i2cControllerPeriph4Transaction.
|
||
//https://www.oshwa.org/2020/06/29/a-resolution-to-redefine-spi-pin-names/
|
||
ICM_20948_Status_e ICM_20948::i2cMasterSLV4Transaction(uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr)
|
||
{
|
||
return (i2cControllerPeriph4Transaction(addr, reg, data, len, Rw, send_reg_addr));
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t data)
|
||
{
|
||
status = ICM_20948_i2c_master_single_w(&_device, addr, reg, &data);
|
||
return status;
|
||
}
|
||
uint8_t ICM_20948::i2cMasterSingleR(uint8_t addr, uint8_t reg)
|
||
{
|
||
uint8_t data = 0;
|
||
status = ICM_20948_i2c_master_single_r(&_device, addr, reg, &data);
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::i2cMasterSingleR: ICM_20948_i2c_master_single_r returned: "));
|
||
debugPrintStatus(status);
|
||
debugPrintln(F(""));
|
||
}
|
||
return data;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::startupDefault(bool minimal)
|
||
{
|
||
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||
|
||
retval = checkID();
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: checkID returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
retval = swReset();
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: swReset returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
delay(50);
|
||
|
||
retval = sleep(false);
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: sleep returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
retval = lowPower(false);
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: lowPower returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
retval = startupMagnetometer(minimal); // Pass the minimal startup flag to startupMagnetometer
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: startupMagnetometer returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
if (minimal) // Return now if minimal is true
|
||
{
|
||
debugPrintln(F("ICM_20948::startupDefault: minimal startup complete!"));
|
||
return status;
|
||
}
|
||
|
||
retval = setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous); // options: ICM_20948_Sample_Mode_Continuous or ICM_20948_Sample_Mode_Cycled
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: setSampleMode returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
} // sensors: ICM_20948_Internal_Acc, ICM_20948_Internal_Gyr, ICM_20948_Internal_Mst
|
||
|
||
ICM_20948_fss_t FSS;
|
||
FSS.a = gpm2; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
|
||
FSS.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
|
||
retval = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS);
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: setFullScale returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_dlpcfg_t dlpcfg;
|
||
dlpcfg.a = acc_d473bw_n499bw;
|
||
dlpcfg.g = gyr_d361bw4_n376bw5;
|
||
retval = setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg);
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: setDLPFcfg returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
retval = enableDLPF(ICM_20948_Internal_Acc, false);
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: enableDLPF (Acc) returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
retval = enableDLPF(ICM_20948_Internal_Gyr, false);
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDefault: enableDLPF (Gyr) returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
return status;
|
||
}
|
||
|
||
// direct read/write
|
||
ICM_20948_Status_e ICM_20948::read(uint8_t reg, uint8_t *pdata, uint32_t len)
|
||
{
|
||
status = ICM_20948_execute_r(&_device, reg, pdata, len);
|
||
return (status);
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::write(uint8_t reg, uint8_t *pdata, uint32_t len)
|
||
{
|
||
status = ICM_20948_execute_w(&_device, reg, pdata, len);
|
||
return (status);
|
||
}
|
||
|
||
uint8_t ICM_20948::readMag(AK09916_Reg_Addr_e reg)
|
||
{
|
||
uint8_t data = i2cMasterSingleR(MAG_AK09916_I2C_ADDR, reg); // i2cMasterSingleR updates status too
|
||
return data;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata)
|
||
{
|
||
status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, reg, *pdata);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::resetMag()
|
||
{
|
||
uint8_t SRST = 1;
|
||
// SRST: Soft reset
|
||
// “0”: Normal
|
||
// “1”: Reset
|
||
// When “1” is set, all registers are initialized. After reset, SRST bit turns to “0” automatically.
|
||
status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL3, SRST);
|
||
return status;
|
||
}
|
||
|
||
// FIFO
|
||
|
||
ICM_20948_Status_e ICM_20948::enableFIFO(bool enable)
|
||
{
|
||
status = ICM_20948_enable_FIFO(&_device, enable);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::resetFIFO(void)
|
||
{
|
||
status = ICM_20948_reset_FIFO(&_device);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::setFIFOmode(bool snapshot)
|
||
{
|
||
// Default to Stream (non-Snapshot) mode
|
||
status = ICM_20948_set_FIFO_mode(&_device, snapshot);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::getFIFOcount(uint16_t *count)
|
||
{
|
||
status = ICM_20948_get_FIFO_count(&_device, count);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::readFIFO(uint8_t *data, uint8_t len)
|
||
{
|
||
status = ICM_20948_read_FIFO(&_device, data, len);
|
||
return status;
|
||
}
|
||
|
||
// DMP
|
||
|
||
ICM_20948_Status_e ICM_20948::enableDMP(bool enable)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to enable the DMP?
|
||
{
|
||
status = ICM_20948_enable_DMP(&_device, enable == true ? 1 : 0);
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::resetDMP(void)
|
||
{
|
||
status = ICM_20948_reset_DMP(&_device);
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::loadDMPFirmware(void)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to load the DMP firmware?
|
||
{
|
||
status = ICM_20948_firmware_load(&_device);
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::setDMPstartAddress(unsigned short address)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to set the start address?
|
||
{
|
||
status = ICM_20948_set_dmp_start_address(&_device, address);
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::enableDMPSensor(enum inv_icm20948_sensor sensor, bool enable)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor?
|
||
{
|
||
status = inv_icm20948_enable_dmp_sensor(&_device, sensor, enable == true ? 1 : 0);
|
||
debugPrint(F("ICM_20948::enableDMPSensor: _enabled_Android_0: "));
|
||
debugPrintf((int)_device._enabled_Android_0);
|
||
debugPrint(F(" _enabled_Android_1: "));
|
||
debugPrintf((int)_device._enabled_Android_1);
|
||
debugPrint(F(" _dataOutCtl1: "));
|
||
debugPrintf((int)_device._dataOutCtl1);
|
||
debugPrint(F(" _dataOutCtl2: "));
|
||
debugPrintf((int)_device._dataOutCtl2);
|
||
debugPrint(F(" _dataRdyStatus: "));
|
||
debugPrintf((int)_device._dataRdyStatus);
|
||
debugPrintln(F(""));
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::enableDMPSensorInt(enum inv_icm20948_sensor sensor, bool enable)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor interrupt?
|
||
{
|
||
status = inv_icm20948_enable_dmp_sensor_int(&_device, sensor, enable == true ? 1 : 0);
|
||
debugPrint(F("ICM_20948::enableDMPSensorInt: _enabled_Android_intr_0: "));
|
||
debugPrintf((int)_device._enabled_Android_intr_0);
|
||
debugPrint(F(" _enabled_Android_intr_1: "));
|
||
debugPrintf((int)_device._enabled_Android_intr_1);
|
||
debugPrint(F(" _dataIntrCtl: "));
|
||
debugPrintf((int)_device._dataIntrCtl);
|
||
debugPrintln(F(""));
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::writeDMPmems(unsigned short reg, unsigned int length, const unsigned char *data)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to write to the DMP?
|
||
{
|
||
status = inv_icm20948_write_mems(&_device, reg, length, data);
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::readDMPmems(unsigned short reg, unsigned int length, unsigned char *data)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to read from the DMP?
|
||
{
|
||
status = inv_icm20948_read_mems(&_device, reg, length, data);
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::setDMPODRrate(enum DMP_ODR_Registers odr_reg, int interval)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to set the DMP ODR?
|
||
{
|
||
// In order to set an ODR for a given sensor data, write 2-byte value to DMP using key defined above for a particular sensor.
|
||
// Setting value can be calculated as follows:
|
||
// Value = (DMP running rate (225Hz) / ODR ) - 1
|
||
// E.g. For a 25Hz ODR rate, value= (225/25) - 1 = 8.
|
||
|
||
status = inv_icm20948_set_dmp_sensor_period(&_device, odr_reg, interval);
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::readDMPdataFromFIFO(icm_20948_DMP_data_t *data)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to set the data from the FIFO?
|
||
{
|
||
status = inv_icm20948_read_dmp_data(&_device, data);
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::setGyroSF(unsigned char div, int gyro_level)
|
||
{
|
||
if (_device._dmp_firmware_available == true) // Should we attempt to set the Gyro SF?
|
||
{
|
||
status = inv_icm20948_set_gyro_sf(&_device, div, gyro_level);
|
||
debugPrint(F("ICM_20948::setGyroSF: pll: "));
|
||
debugPrintf((int)_device._gyroSFpll);
|
||
debugPrint(F(" Gyro SF is: "));
|
||
debugPrintf((int)_device._gyroSF);
|
||
debugPrintln(F(""));
|
||
return status;
|
||
}
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
// Combine all of the DMP start-up code from the earlier DMP examples
|
||
// This function is defined as __attribute__((weak)) so you can overwrite it if you want to,
|
||
// e.g. to modify the sample rate
|
||
ICM_20948_Status_e ICM_20948::initializeDMP(void)
|
||
{
|
||
// First, let's check if the DMP is available
|
||
if (_device._dmp_firmware_available != true)
|
||
{
|
||
debugPrint(F("ICM_20948::startupDMP: DMP is not available. Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h..."));
|
||
return ICM_20948_Stat_DMPNotSupported;
|
||
}
|
||
|
||
ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok;
|
||
|
||
#if defined(ICM_20948_USE_DMP)
|
||
|
||
// The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration
|
||
// sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
|
||
|
||
ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful
|
||
|
||
// Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, which is called by begin) configures the AK09916 magnetometer
|
||
// to run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's I2C_SLV0 is configured to read
|
||
// nine bytes from the mag every sample, starting from the STATUS1 register (0x10). ST1 includes the DRDY (Data Ready) bit.
|
||
// Next are the six magnetometer readings (little endian). After a dummy byte, the STATUS2 register (0x18) contains the HOFL (Overflow) bit.
|
||
//
|
||
// But looking very closely at the InvenSense example code, we can see in inv_icm20948_resume_akm (in Icm20948AuxCompassAkm.c) that,
|
||
// when the DMP is running, the magnetometer is set to Single Measurement (SM) mode and that ten bytes are read, starting from the reserved
|
||
// RSV2 register (0x03). The datasheet does not define what registers 0x04 to 0x0C contain. There is definitely some secret sauce in here...
|
||
// The magnetometer data appears to be big endian (not little endian like the HX/Y/Z registers) and starts at register 0x04.
|
||
// We had to examine the I2C traffic between the master and the AK09916 on the AUX_DA and AUX_CL pins to discover this...
|
||
//
|
||
// So, we need to set up I2C_SLV0 to do the ten byte reading. The parameters passed to i2cControllerConfigurePeripheral are:
|
||
// 0: use I2C_SLV0
|
||
// MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted)
|
||
// AK09916_REG_RSV2: we start reading here (0x03). Secret sauce...
|
||
// 10: we read 10 bytes each cycle
|
||
// true: set the I2C_SLV0_RNW ReadNotWrite bit so we read the 10 bytes (not write them)
|
||
// true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit to enable reading from the peripheral at the sample rate
|
||
// false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value)
|
||
// true: set the I2C_SLV0_CTRL I2C_SLV0_GRP bit to show the register pairing starts at byte 1+2 (copied from inv_icm20948_resume_akm)
|
||
// true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag (copied from inv_icm20948_resume_akm)
|
||
result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true); if (result > worstResult) worstResult = result;
|
||
//
|
||
// We also need to set up I2C_SLV1 to do the Single Measurement triggering:
|
||
// 1: use I2C_SLV1
|
||
// MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted)
|
||
// AK09916_REG_CNTL2: we start writing here (0x31)
|
||
// 1: not sure why, but the write does not happen if this is set to zero
|
||
// false: clear the I2C_SLV0_RNW ReadNotWrite bit so we write the dataOut byte
|
||
// true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit. Not sure why, but the write does not happen if this is clear
|
||
// false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value)
|
||
// false: clear the I2C_SLV0_CTRL I2C_SLV0_GRP bit
|
||
// false: clear the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW bit
|
||
// AK09916_mode_single: tell I2C_SLV1 to write the Single Measurement command each sample
|
||
result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single); if (result > worstResult) worstResult = result;
|
||
|
||
// Set the I2C Master ODR configuration
|
||
// It is not clear why we need to do this... But it appears to be essential! From the datasheet:
|
||
// "I2C_MST_ODR_CONFIG[3:0]: ODR configuration for external sensor when gyroscope and accelerometer are disabled.
|
||
// ODR is computed as follows: 1.1 kHz/(2^((odr_config[3:0])) )
|
||
// When gyroscope is enabled, all sensors (including I2C_MASTER) use the gyroscope ODR.
|
||
// If gyroscope is disabled, then all sensors (including I2C_MASTER) use the accelerometer ODR."
|
||
// Since both gyro and accel are running, setting this register should have no effect. But it does. Maybe because the Gyro and Accel are placed in Low Power Mode (cycled)?
|
||
// You can see by monitoring the Aux I2C pins that the next three lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen rate: 68.75Hz in this case.
|
||
result = setBank(3); if (result > worstResult) worstResult = result; // Select Bank 3
|
||
uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz
|
||
result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); if (result > worstResult) worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register
|
||
|
||
// Configure clock source through PWR_MGMT_1
|
||
// ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator
|
||
result = setClockSource(ICM_20948_Clock_Auto); if (result > worstResult) worstResult = result; // This is shorthand: success will be set to false if setClockSource fails
|
||
|
||
// Enable accel and gyro sensors through PWR_MGMT_2
|
||
// Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2
|
||
result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
|
||
uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?)
|
||
result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register
|
||
|
||
// Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG
|
||
// The InvenSense Nucleo example initially puts the accel and gyro into low power mode too, but then later updates LP_CONFIG so only the I2C_Master is in Low Power Mode
|
||
result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result;
|
||
|
||
// Disable the FIFO
|
||
result = enableFIFO(false); if (result > worstResult) worstResult = result;
|
||
|
||
// Disable the DMP
|
||
result = enableDMP(false); if (result > worstResult) worstResult = result;
|
||
|
||
// Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1
|
||
// Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG
|
||
ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors
|
||
myFSS.a = gpm4; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
|
||
// gpm2
|
||
// gpm4
|
||
// gpm8
|
||
// gpm16
|
||
myFSS.g = dps2000; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
|
||
// dps250
|
||
// dps500
|
||
// dps1000
|
||
// dps2000
|
||
result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); if (result > worstResult) worstResult = result;
|
||
|
||
// The InvenSense Nucleo code also enables the gyro DLPF (but leaves GYRO_DLPFCFG set to zero = 196.6Hz (3dB))
|
||
// We found this by going through the SPI data generated by ZaneL's Teensy-ICM-20948 library byte by byte...
|
||
// The gyro DLPF is enabled by default (GYRO_CONFIG_1 = 0x01) so the following line should have no effect, but we'll include it anyway
|
||
result = enableDLPF(ICM_20948_Internal_Gyr, true); if (result > worstResult) worstResult = result;
|
||
|
||
// Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2
|
||
// If we see this interrupt, we'll need to reset the FIFO
|
||
//result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs
|
||
|
||
// Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2
|
||
// Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1
|
||
result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
|
||
uint8_t zero = 0;
|
||
result = write(AGB0_REG_FIFO_EN_1, &zero, 1); if (result > worstResult) worstResult = result;
|
||
// Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2
|
||
result = write(AGB0_REG_FIFO_EN_2, &zero, 1); if (result > worstResult) worstResult = result;
|
||
|
||
// Turn off data ready interrupt through INT_ENABLE_1
|
||
result = intEnableRawDataReady(false); if (result > worstResult) worstResult = result;
|
||
|
||
// Reset FIFO through FIFO_RST
|
||
result = resetFIFO(); if (result > worstResult) worstResult = result;
|
||
|
||
// Set gyro sample rate divider with GYRO_SMPLRT_DIV
|
||
// Set accel sample rate divider with ACCEL_SMPLRT_DIV_2
|
||
ICM_20948_smplrt_t mySmplrt;
|
||
mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13).
|
||
mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13).
|
||
//mySmplrt.g = 4; // 225Hz
|
||
//mySmplrt.a = 4; // 225Hz
|
||
//mySmplrt.g = 8; // 112Hz
|
||
//mySmplrt.a = 8; // 112Hz
|
||
result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result;
|
||
|
||
// Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
|
||
result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS
|
||
|
||
// Now load the DMP firmware
|
||
result = loadDMPFirmware(); if (result > worstResult) worstResult = result;
|
||
|
||
// Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
|
||
result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS
|
||
|
||
// Set the Hardware Fix Disable register to 0x48
|
||
result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
|
||
uint8_t fix = 0x48;
|
||
result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); if (result > worstResult) worstResult = result;
|
||
|
||
// Set the Single FIFO Priority Select register to 0xE4
|
||
result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
|
||
uint8_t fifoPrio = 0xE4;
|
||
result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); if (result > worstResult) worstResult = result;
|
||
|
||
// Configure Accel scaling to DMP
|
||
// The DMP scales accel raw data internally to align 1g as 2^25
|
||
// In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is 4g
|
||
const unsigned char accScale[4] = {0x04, 0x00, 0x00, 0x00};
|
||
result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); if (result > worstResult) worstResult = result; // Write accScale to ACC_SCALE DMP register
|
||
// In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g
|
||
const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00};
|
||
result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); if (result > worstResult) worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register
|
||
|
||
// Configure Compass mount matrix and scale to DMP
|
||
// The mount matrix write to DMP register is used to align the compass axes with accel/gyro.
|
||
// This mechanism is also used to convert hardware unit to uT. The value is expressed as 1uT = 2^30.
|
||
// Each compass axis will be converted as below:
|
||
// X = raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02
|
||
// Y = raw_x * CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12
|
||
// Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22
|
||
// The AK09916 produces a 16-bit signed output in the range +/-32752 corresponding to +/-4912uT. 1uT = 6.66 ADU.
|
||
// 2^30 / 6.66666 = 161061273 = 0x9999999
|
||
const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
|
||
const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99}; // Value taken from InvenSense Nucleo example
|
||
const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example
|
||
result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result;
|
||
|
||
// Configure the B2S Mounting Matrix
|
||
const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
|
||
const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
|
||
result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
|
||
result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
|
||
|
||
// Configure the DMP Gyro Scaling Factor
|
||
// @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where
|
||
// 0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, ...
|
||
// 10=102.2727Hz sample rate, ... etc.
|
||
// @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps
|
||
result = setGyroSF(19, 3); if (result > worstResult) worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above)
|
||
|
||
// Configure the Gyro full scale
|
||
// 2000dps : 2^28
|
||
// 1000dps : 2^27
|
||
// 500dps : 2^26
|
||
// 250dps : 2^25
|
||
const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28
|
||
result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); if (result > worstResult) worstResult = result;
|
||
|
||
// Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
|
||
const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz
|
||
//const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz
|
||
//const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz
|
||
result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); if (result > worstResult) worstResult = result;
|
||
|
||
// Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
|
||
const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz
|
||
//const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz
|
||
//const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz
|
||
result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); if (result > worstResult) worstResult = result;
|
||
|
||
// Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
|
||
const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz
|
||
//const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz
|
||
//const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz
|
||
result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); if (result > worstResult) worstResult = result;
|
||
|
||
// Configure the Accel Cal Rate
|
||
const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example
|
||
result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); if (result > worstResult) worstResult = result;
|
||
|
||
// Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) sets the magnetometer read rate to 68.75Hz.
|
||
// Let's set the Compass Time Buffer to 69 (Hz).
|
||
const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz
|
||
result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); if (result > worstResult) worstResult = result;
|
||
|
||
// Enable DMP interrupt
|
||
// This would be the most efficient way of getting the DMP data, instead of polling the FIFO
|
||
//result = intEnableDMP(true); if (result > worstResult) worstResult = result;
|
||
|
||
#endif
|
||
|
||
return worstResult;
|
||
}
|
||
|
||
// I2C
|
||
ICM_20948_I2C::ICM_20948_I2C()
|
||
{
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t ad0pin)
|
||
{
|
||
// Associate
|
||
_ad0 = ad0pin;
|
||
_i2c = &wirePort;
|
||
_ad0val = ad0val;
|
||
|
||
_addr = ICM_20948_I2C_ADDR_AD0;
|
||
if (_ad0val)
|
||
{
|
||
_addr = ICM_20948_I2C_ADDR_AD1;
|
||
}
|
||
|
||
// Set pinmodes
|
||
if (_ad0 != ICM_20948_ARD_UNUSED_PIN)
|
||
{
|
||
pinMode(_ad0, OUTPUT);
|
||
}
|
||
|
||
// Set pins to default positions
|
||
if (_ad0 != ICM_20948_ARD_UNUSED_PIN)
|
||
{
|
||
digitalWrite(_ad0, _ad0val);
|
||
}
|
||
|
||
// _i2c->begin(); // Moved into user's sketch
|
||
|
||
// Set up the serif
|
||
_serif.write = ICM_20948_write_I2C;
|
||
_serif.read = ICM_20948_read_I2C;
|
||
_serif.user = (void *)this; // refer to yourself in the user field
|
||
|
||
// Link the serif
|
||
_device._serif = &_serif;
|
||
|
||
#if defined(ICM_20948_USE_DMP)
|
||
_device._dmp_firmware_available = true; // Initialize _dmp_firmware_available
|
||
#else
|
||
_device._dmp_firmware_available = false; // Initialize _dmp_firmware_available
|
||
#endif
|
||
|
||
_device._firmware_loaded = false; // Initialize _firmware_loaded
|
||
_device._last_bank = 255; // Initialize _last_bank. Make it invalid. It will be set by the first call of ICM_20948_set_bank.
|
||
_device._last_mems_bank = 255; // Initialize _last_mems_bank. Make it invalid. It will be set by the first call of inv_icm20948_write_mems.
|
||
_device._gyroSF = 0; // Use this to record the GyroSF, calculated by inv_icm20948_set_gyro_sf
|
||
_device._gyroSFpll = 0;
|
||
_device._enabled_Android_0 = 0; // Keep track of which Android sensors are enabled: 0-31
|
||
_device._enabled_Android_1 = 0; // Keep track of which Android sensors are enabled: 32-
|
||
_device._enabled_Android_intr_0 = 0; // Keep track of which Android sensor interrupts are enabled: 0-31
|
||
_device._enabled_Android_intr_1 = 0; // Keep track of which Android sensor interrupts are enabled: 32-
|
||
|
||
// Perform default startup
|
||
// Do a minimal startupDefault if using the DMP. User can always call startupDefault(false) manually if required.
|
||
status = startupDefault(_device._dmp_firmware_available);
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948_I2C::begin: startupDefault returned: "));
|
||
debugPrintStatus(status);
|
||
debugPrintln(F(""));
|
||
}
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::startupMagnetometer(bool minimal)
|
||
{
|
||
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||
|
||
i2cMasterPassthrough(false); //Do not connect the SDA/SCL pins to AUX_DA/AUX_CL
|
||
i2cMasterEnable(true);
|
||
|
||
resetMag();
|
||
|
||
//After a ICM reset the Mag sensor may stop responding over the I2C master
|
||
//Reset the Master I2C until it responds
|
||
uint8_t tries = 0;
|
||
while (tries < MAX_MAGNETOMETER_STARTS)
|
||
{
|
||
tries++;
|
||
|
||
//See if we can read the WhoIAm register correctly
|
||
retval = magWhoIAm();
|
||
if (retval == ICM_20948_Stat_Ok)
|
||
break; //WIA matched!
|
||
|
||
i2cMasterReset(); //Otherwise, reset the master I2C and try again
|
||
|
||
delay(10);
|
||
}
|
||
|
||
if (tries == MAX_MAGNETOMETER_STARTS)
|
||
{
|
||
debugPrint(F("ICM_20948::startupMagnetometer: reached MAX_MAGNETOMETER_STARTS ("));
|
||
debugPrintf((int)MAX_MAGNETOMETER_STARTS);
|
||
debugPrintln(F("). Returning ICM_20948_Stat_WrongID"));
|
||
status = ICM_20948_Stat_WrongID;
|
||
return status;
|
||
}
|
||
else
|
||
{
|
||
debugPrint(F("ICM_20948::startupMagnetometer: successful magWhoIAm after "));
|
||
debugPrintf((int)tries);
|
||
if (tries == 1)
|
||
debugPrintln(F(" try"));
|
||
else
|
||
debugPrintln(F(" tries"));
|
||
}
|
||
|
||
//Return now if minimal is true. The mag will be configured manually for the DMP
|
||
if (minimal) // Return now if minimal is true
|
||
{
|
||
debugPrintln(F("ICM_20948::startupMagnetometer: minimal startup complete!"));
|
||
return status;
|
||
}
|
||
|
||
//Set up magnetometer
|
||
AK09916_CNTL2_Reg_t reg;
|
||
reg.MODE = AK09916_mode_cont_100hz;
|
||
reg.reserved_0 = 0; // Make sure the unused bits are clear. Probably redundant, but prevents confusion when looking at the I2C traffic
|
||
retval = writeMag(AK09916_REG_CNTL2, (uint8_t *)®);
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupMagnetometer: writeMag returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
retval = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_ST1, 9, true, true, false, false, false);
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::startupMagnetometer: i2cMasterConfigurePeripheral returned: "));
|
||
debugPrintStatus(retval);
|
||
debugPrintln(F(""));
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
return status;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948::magWhoIAm(void)
|
||
{
|
||
ICM_20948_Status_e retval = ICM_20948_Stat_Ok;
|
||
|
||
uint8_t whoiam1, whoiam2;
|
||
whoiam1 = readMag(AK09916_REG_WIA1);
|
||
// readMag calls i2cMasterSingleR which calls ICM_20948_i2c_master_single_r
|
||
// i2cMasterSingleR updates status so it is OK to set retval to status here
|
||
retval = status;
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::magWhoIAm: whoiam1: "));
|
||
debugPrintf((int)whoiam1);
|
||
debugPrint(F(" (should be 72) readMag set status to: "));
|
||
debugPrintStatus(status);
|
||
debugPrintln(F(""));
|
||
return retval;
|
||
}
|
||
whoiam2 = readMag(AK09916_REG_WIA2);
|
||
// readMag calls i2cMasterSingleR which calls ICM_20948_i2c_master_single_r
|
||
// i2cMasterSingleR updates status so it is OK to set retval to status here
|
||
retval = status;
|
||
if (retval != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948::magWhoIAm: whoiam1: "));
|
||
debugPrintf((int)whoiam1);
|
||
debugPrint(F(" (should be 72) whoiam2: "));
|
||
debugPrintf((int)whoiam2);
|
||
debugPrint(F(" (should be 9) readMag set status to: "));
|
||
debugPrintStatus(status);
|
||
debugPrintln(F(""));
|
||
return retval;
|
||
}
|
||
|
||
if ((whoiam1 == (MAG_AK09916_WHO_AM_I >> 8)) && (whoiam2 == (MAG_AK09916_WHO_AM_I & 0xFF)))
|
||
{
|
||
retval = ICM_20948_Stat_Ok;
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
debugPrint(F("ICM_20948::magWhoIAm: whoiam1: "));
|
||
debugPrintf((int)whoiam1);
|
||
debugPrint(F(" (should be 72) whoiam2: "));
|
||
debugPrintf((int)whoiam2);
|
||
debugPrintln(F(" (should be 9). Returning ICM_20948_Stat_WrongID"));
|
||
|
||
retval = ICM_20948_Stat_WrongID;
|
||
status = retval;
|
||
return status;
|
||
}
|
||
|
||
// SPI
|
||
|
||
// SPISettings ICM_20948_SPI_DEFAULT_SETTINGS(ICM_20948_SPI_DEFAULT_FREQ, ICM_20948_SPI_DEFAULT_ORDER, ICM_20948_SPI_DEFAULT_MODE);
|
||
|
||
ICM_20948_SPI::ICM_20948_SPI()
|
||
{
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948_SPI::begin(uint8_t csPin, SPIClass &spiPort, uint32_t SPIFreq)
|
||
{
|
||
if (SPIFreq > 7000000)
|
||
SPIFreq = 7000000; // Limit SPI frequency to 7MHz
|
||
|
||
// Associate
|
||
_spi = &spiPort;
|
||
_spisettings = SPISettings(SPIFreq, ICM_20948_SPI_DEFAULT_ORDER, ICM_20948_SPI_DEFAULT_MODE);
|
||
_cs = csPin;
|
||
|
||
// Set pinmodes
|
||
pinMode(_cs, OUTPUT);
|
||
|
||
// Set pins to default positions
|
||
digitalWrite(_cs, HIGH);
|
||
|
||
// _spi->begin(); // Moved into user's sketch
|
||
|
||
// 'Kickstart' the SPI hardware.
|
||
_spi->beginTransaction(_spisettings);
|
||
_spi->transfer(0x00);
|
||
_spi->endTransaction();
|
||
|
||
// Set up the serif
|
||
_serif.write = ICM_20948_write_SPI;
|
||
_serif.read = ICM_20948_read_SPI;
|
||
_serif.user = (void *)this; // refer to yourself in the user field
|
||
|
||
// Link the serif
|
||
_device._serif = &_serif;
|
||
|
||
#if defined(ICM_20948_USE_DMP)
|
||
_device._dmp_firmware_available = true; // Initialize _dmp_firmware_available
|
||
#else
|
||
_device._dmp_firmware_available = false; // Initialize _dmp_firmware_available
|
||
#endif
|
||
|
||
_device._firmware_loaded = false; // Initialize _firmware_loaded
|
||
_device._last_bank = 255; // Initialize _last_bank. Make it invalid. It will be set by the first call of ICM_20948_set_bank.
|
||
_device._last_mems_bank = 255; // Initialize _last_mems_bank. Make it invalid. It will be set by the first call of inv_icm20948_write_mems.
|
||
_device._gyroSF = 0; // Use this to record the GyroSF, calculated by inv_icm20948_set_gyro_sf
|
||
_device._gyroSFpll = 0;
|
||
_device._enabled_Android_0 = 0; // Keep track of which Android sensors are enabled: 0-31
|
||
_device._enabled_Android_1 = 0; // Keep track of which Android sensors are enabled: 32-
|
||
_device._enabled_Android_intr_0 = 0; // Keep track of which Android sensor interrupts are enabled: 0-31
|
||
_device._enabled_Android_intr_1 = 0; // Keep track of which Android sensor interrupts are enabled: 32-
|
||
|
||
// Perform default startup
|
||
// Do a minimal startupDefault if using the DMP. User can always call startupDefault(false) manually if required.
|
||
status = startupDefault(_device._dmp_firmware_available);
|
||
if (status != ICM_20948_Stat_Ok)
|
||
{
|
||
debugPrint(F("ICM_20948_SPI::begin: startupDefault returned: "));
|
||
debugPrintStatus(status);
|
||
debugPrintln(F(""));
|
||
}
|
||
|
||
return status;
|
||
}
|
||
|
||
// serif functions for the I2C and SPI classes
|
||
ICM_20948_Status_e ICM_20948_write_I2C(uint8_t reg, uint8_t *data, uint32_t len, void *user)
|
||
{
|
||
if (user == NULL)
|
||
{
|
||
return ICM_20948_Stat_ParamErr;
|
||
}
|
||
TwoWire *_i2c = ((ICM_20948_I2C *)user)->_i2c; // Cast user field to ICM_20948_I2C type and extract the I2C interface pointer
|
||
uint8_t addr = ((ICM_20948_I2C *)user)->_addr;
|
||
if (_i2c == NULL)
|
||
{
|
||
return ICM_20948_Stat_ParamErr;
|
||
}
|
||
|
||
_i2c->beginTransmission(addr);
|
||
_i2c->write(reg);
|
||
_i2c->write(data, (uint8_t)len);
|
||
_i2c->endTransmission();
|
||
|
||
// for( uint32_t indi = 0; indi < len; indi++ ){
|
||
// _i2c->beginTransmission(addr);
|
||
// _i2c->write(reg + indi);
|
||
// _i2c->write(*(data + indi) );
|
||
// _i2c->endTransmission();
|
||
// delay(10);
|
||
// }
|
||
|
||
// delay(10);
|
||
|
||
return ICM_20948_Stat_Ok;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948_read_I2C(uint8_t reg, uint8_t *buff, uint32_t len, void *user)
|
||
{
|
||
if (user == NULL)
|
||
{
|
||
return ICM_20948_Stat_ParamErr;
|
||
}
|
||
TwoWire *_i2c = ((ICM_20948_I2C *)user)->_i2c;
|
||
uint8_t addr = ((ICM_20948_I2C *)user)->_addr;
|
||
if (_i2c == NULL)
|
||
{
|
||
return ICM_20948_Stat_ParamErr;
|
||
}
|
||
|
||
_i2c->beginTransmission(addr);
|
||
_i2c->write(reg);
|
||
_i2c->endTransmission(false); // Send repeated start
|
||
|
||
uint32_t num_received = _i2c->requestFrom(addr, len);
|
||
|
||
if (num_received == len)
|
||
{
|
||
for (uint8_t i = 0; i < len; i++)
|
||
{
|
||
buff[i] = _i2c->read();
|
||
}
|
||
return ICM_20948_Stat_Ok;
|
||
}
|
||
else
|
||
{
|
||
return ICM_20948_Stat_NoData;
|
||
}
|
||
|
||
if (len != 0)
|
||
{
|
||
return ICM_20948_Stat_NoData;
|
||
}
|
||
return ICM_20948_Stat_Ok;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948_write_SPI(uint8_t reg, uint8_t *data, uint32_t len, void *user)
|
||
{
|
||
if (user == NULL)
|
||
{
|
||
return ICM_20948_Stat_ParamErr;
|
||
}
|
||
SPIClass *_spi = ((ICM_20948_SPI *)user)->_spi; // Cast user field to ICM_20948_SPI type and extract the SPI interface pointer
|
||
uint8_t cs = ((ICM_20948_SPI *)user)->_cs;
|
||
SPISettings spisettings = ((ICM_20948_SPI *)user)->_spisettings;
|
||
if (_spi == NULL)
|
||
{
|
||
return ICM_20948_Stat_ParamErr;
|
||
}
|
||
|
||
// 'Kickstart' the SPI hardware. This is a fairly high amount of overhead, but it guarantees that the lines will start in the correct states even when sharing the SPI bus with devices that use other modes
|
||
_spi->beginTransaction(spisettings);
|
||
_spi->transfer(0x00);
|
||
_spi->endTransaction();
|
||
|
||
_spi->beginTransaction(spisettings);
|
||
digitalWrite(cs, LOW);
|
||
// delayMicroseconds(5);
|
||
_spi->transfer(((reg & 0x7F) | 0x00));
|
||
// SPI.transfer(data, len); // Can't do this thanks to Arduino's poor implementation
|
||
for (uint32_t indi = 0; indi < len; indi++)
|
||
{
|
||
_spi->transfer(*(data + indi));
|
||
}
|
||
// delayMicroseconds(5);
|
||
digitalWrite(cs, HIGH);
|
||
_spi->endTransaction();
|
||
|
||
return ICM_20948_Stat_Ok;
|
||
}
|
||
|
||
ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user)
|
||
{
|
||
if (user == NULL)
|
||
{
|
||
return ICM_20948_Stat_ParamErr;
|
||
}
|
||
SPIClass *_spi = ((ICM_20948_SPI *)user)->_spi;
|
||
uint8_t cs = ((ICM_20948_SPI *)user)->_cs;
|
||
SPISettings spisettings = ((ICM_20948_SPI *)user)->_spisettings;
|
||
if (_spi == NULL)
|
||
{
|
||
return ICM_20948_Stat_ParamErr;
|
||
}
|
||
|
||
// 'Kickstart' the SPI hardware. This is a fairly high amount of overhead, but it guarantees that the lines will start in the correct states
|
||
_spi->beginTransaction(spisettings);
|
||
_spi->transfer(0x00);
|
||
_spi->endTransaction();
|
||
|
||
_spi->beginTransaction(spisettings);
|
||
digitalWrite(cs, LOW);
|
||
// delayMicroseconds(5);
|
||
_spi->transfer(((reg & 0x7F) | 0x80));
|
||
// SPI.transfer(data, len); // Can't do this thanks to Arduino's stupid implementation
|
||
for (uint32_t indi = 0; indi < len; indi++)
|
||
{
|
||
*(buff + indi) = _spi->transfer(0x00);
|
||
}
|
||
// delayMicroseconds(5);
|
||
digitalWrite(cs, HIGH);
|
||
_spi->endTransaction();
|
||
|
||
return ICM_20948_Stat_Ok;
|
||
}
|
||
|
||
|