Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Added Bias saving and loading. #105

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Nov 14, 2022
Merged

Added Bias saving and loading. #105

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
192 changes: 192 additions & 0 deletions src/ICM_20948.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -239,6 +239,196 @@ float ICM_20948::getGyrDPS(int16_t axis_val)
}
}

//Gyro Bias
ICM_20948_Status_e ICM_20948::SetBiasGyroX( int32_t 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( int32_t 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( int32_t 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( int32_t* 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] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
return result;
}

ICM_20948_Status_e ICM_20948::GetBiasGyroY( int32_t* 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] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
return result;
}

ICM_20948_Status_e ICM_20948::GetBiasGyroZ( int32_t* 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] = (int32_t)(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( int32_t 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( int32_t 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( int32_t 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( int32_t* 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] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
return result;
}

ICM_20948_Status_e ICM_20948::GetBiasAccelY( int32_t* 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] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
return result;
}

ICM_20948_Status_e ICM_20948::GetBiasAccelZ( int32_t* 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] = (int32_t)(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( int32_t 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( int32_t 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( int32_t 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( int32_t* 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] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
return result;
}

ICM_20948_Status_e ICM_20948::GetBiasCPassY( int32_t* 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] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
return result;
}

ICM_20948_Status_e ICM_20948::GetBiasCPassZ( int32_t* 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] = (int32_t)(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);
Expand Down Expand Up @@ -1796,3 +1986,5 @@ ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len,

return ICM_20948_Stat_Ok;
}


22 changes: 22 additions & 0 deletions src/ICM_20948.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -173,6 +173,28 @@ class ICM_20948

//DMP

//Gyro Bias
ICM_20948_Status_e SetBiasGyroX( int32_t newValue);
ICM_20948_Status_e SetBiasGyroY( int32_t newValue);
ICM_20948_Status_e SetBiasGyroZ( int32_t newValue);
ICM_20948_Status_e GetBiasGyroX( int32_t* bias);
ICM_20948_Status_e GetBiasGyroY( int32_t* bias);
ICM_20948_Status_e GetBiasGyroZ( int32_t* bias);
//Accel Bias
ICM_20948_Status_e SetBiasAccelX( int32_t newValue);
ICM_20948_Status_e SetBiasAccelY( int32_t newValue);
ICM_20948_Status_e SetBiasAccelZ( int32_t newValue);
ICM_20948_Status_e GetBiasAccelX( int32_t* bias);
ICM_20948_Status_e GetBiasAccelY( int32_t* bias);
ICM_20948_Status_e GetBiasAccelZ( int32_t* bias);
//CPass Bias
ICM_20948_Status_e SetBiasCPassX( int32_t newValue);
ICM_20948_Status_e SetBiasCPassY( int32_t newValue);
ICM_20948_Status_e SetBiasCPassZ( int32_t newValue);
ICM_20948_Status_e GetBiasCPassX( int32_t* bias);
ICM_20948_Status_e GetBiasCPassY( int32_t* bias);
ICM_20948_Status_e GetBiasCPassZ( int32_t* bias);

// Done:
// Configure DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
// Load Firmware
Expand Down
6 changes: 4 additions & 2 deletions src/util/ICM_20948_C.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@

#if defined(ARDUINO_ARCH_MBED) // ARDUINO_ARCH_MBED (APOLLO3 v2) does not support or require pgmspace.h / PROGMEM
const uint8_t dmp3_image[] = {
#elif (defined(__AVR__) || defined(__arm__) || defined(__ARDUINO_ARC__)) && !defined(__linux__) // Store the DMP firmware in PROGMEM on older AVR (ATmega) platforms
#elif (defined(__AVR__) || defined(__arm__) || defined(__ARDUINO_ARC__) || defined(ESP8266)) && !defined(__linux__) // Store the DMP firmware in PROGMEM on older AVR (ATmega) platforms
#define ICM_20948_USE_PROGMEM_FOR_DMP
#include <avr/pgmspace.h>
const uint8_t dmp3_image[] PROGMEM = {
Expand Down Expand Up @@ -2589,7 +2589,9 @@ ICM_20948_Status_e inv_icm20948_set_gyro_sf(ICM_20948_Device_t *pdev, unsigned c
gyro_sf_reg[1] = (unsigned char)(gyro_sf >> 16);
gyro_sf_reg[2] = (unsigned char)(gyro_sf >> 8);
gyro_sf_reg[3] = (unsigned char)(gyro_sf & 0xff);
result = inv_icm20948_write_mems(pdev, GYRO_SF, 4, (const unsigned char *)&gyro_sf_reg);
result = inv_icm20948_write_mems(pdev, GYRO_SF, 4, (const unsigned char*)&gyro_sf_reg);

return result;
}