Skip to content

Commit c8fa3c4

Browse files
ThreadOfFateThreadOfFate
committed
Added Bias saving and loading.
1 parent d5ae1eb commit c8fa3c4

File tree

3 files changed

+218
-2
lines changed

3 files changed

+218
-2
lines changed

‎src/ICM_20948.cpp

Lines changed: 192 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -239,6 +239,196 @@ float ICM_20948::getGyrDPS(int16_t axis_val)
239239
}
240240
}
241241

242+
//Gyro Bias
243+
ICM_20948_Status_e ICM_20948::SetBiasGyroX( int32_t newValue)
244+
{
245+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
246+
unsigned char gyro_bias_reg[4];
247+
gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
248+
gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
249+
gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
250+
gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
251+
result = inv_icm20948_write_mems(&_device, GYRO_BIAS_X, 4, (const unsigned char*)&gyro_bias_reg);
252+
return result;
253+
}
254+
255+
ICM_20948_Status_e ICM_20948::SetBiasGyroY( int32_t newValue)
256+
{
257+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
258+
unsigned char gyro_bias_reg[4];
259+
gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
260+
gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
261+
gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
262+
gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
263+
result = inv_icm20948_write_mems(&_device, GYRO_BIAS_Y, 4, (const unsigned char*)&gyro_bias_reg);
264+
return result;
265+
}
266+
267+
ICM_20948_Status_e ICM_20948::SetBiasGyroZ( int32_t newValue)
268+
{
269+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
270+
unsigned char gyro_bias_reg[4];
271+
gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
272+
gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
273+
gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
274+
gyro_bias_reg[3] = (unsigned char)(newValue & 0xff);
275+
result = inv_icm20948_write_mems(&_device, GYRO_BIAS_Z, 4, (const unsigned char*)&gyro_bias_reg);
276+
return result;
277+
}
278+
279+
ICM_20948_Status_e ICM_20948::GetBiasGyroX( int32_t* bias)
280+
{
281+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
282+
unsigned char bias_data[4] = { 0 };
283+
result = inv_icm20948_read_mems(&_device, GYRO_BIAS_X, 4, bias_data);
284+
bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
285+
return result;
286+
}
287+
288+
ICM_20948_Status_e ICM_20948::GetBiasGyroY( int32_t* bias)
289+
{
290+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
291+
unsigned char bias_data[4] = { 0 };
292+
result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Y, 4, bias_data);
293+
bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
294+
return result;
295+
}
296+
297+
ICM_20948_Status_e ICM_20948::GetBiasGyroZ( int32_t* bias)
298+
{
299+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
300+
unsigned char bias_data[4] = { 0 };
301+
result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Z, 4, bias_data);
302+
bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
303+
return result;
304+
}
305+
//Accel Bias
306+
ICM_20948_Status_e ICM_20948::SetBiasAccelX( int32_t newValue)
307+
{
308+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
309+
unsigned char accel_bias_reg[4];
310+
accel_bias_reg[0] = (unsigned char)(newValue >> 24);
311+
accel_bias_reg[1] = (unsigned char)(newValue >> 16);
312+
accel_bias_reg[2] = (unsigned char)(newValue >> 8);
313+
accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
314+
result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_X, 4, (const unsigned char*)&accel_bias_reg);
315+
return result;
316+
}
317+
318+
ICM_20948_Status_e ICM_20948::SetBiasAccelY( int32_t newValue)
319+
{
320+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
321+
unsigned char accel_bias_reg[4];
322+
accel_bias_reg[0] = (unsigned char)(newValue >> 24);
323+
accel_bias_reg[1] = (unsigned char)(newValue >> 16);
324+
accel_bias_reg[2] = (unsigned char)(newValue >> 8);
325+
accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
326+
result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Y, 4, (const unsigned char*)&accel_bias_reg);
327+
return result;
328+
}
329+
330+
ICM_20948_Status_e ICM_20948::SetBiasAccelZ( int32_t newValue)
331+
{
332+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
333+
unsigned char accel_bias_reg[4];
334+
accel_bias_reg[0] = (unsigned char)(newValue >> 24);
335+
accel_bias_reg[1] = (unsigned char)(newValue >> 16);
336+
accel_bias_reg[2] = (unsigned char)(newValue >> 8);
337+
accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
338+
result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Z, 4, (const unsigned char*)&accel_bias_reg);
339+
return result;
340+
}
341+
342+
ICM_20948_Status_e ICM_20948::GetBiasAccelX( int32_t* bias)
343+
{
344+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
345+
unsigned char bias_data[4] = { 0 };
346+
result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_X, 4, bias_data);
347+
bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
348+
return result;
349+
}
350+
351+
ICM_20948_Status_e ICM_20948::GetBiasAccelY( int32_t* bias)
352+
{
353+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
354+
unsigned char bias_data[4] = { 0 };
355+
result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Y, 4, bias_data);
356+
bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
357+
return result;
358+
}
359+
360+
ICM_20948_Status_e ICM_20948::GetBiasAccelZ( int32_t* bias)
361+
{
362+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
363+
unsigned char bias_data[4] = { 0 };
364+
result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Z, 4, bias_data);
365+
bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
366+
return result;
367+
}
368+
//CPass Bias
369+
ICM_20948_Status_e ICM_20948::SetBiasCPassX( int32_t newValue)
370+
{
371+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
372+
unsigned char cpass_bias_reg[4];
373+
cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
374+
cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
375+
cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
376+
cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
377+
result = inv_icm20948_write_mems(&_device, CPASS_BIAS_X, 4, (const unsigned char*)&cpass_bias_reg);
378+
return result;
379+
}
380+
381+
ICM_20948_Status_e ICM_20948::SetBiasCPassY( int32_t newValue)
382+
{
383+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
384+
unsigned char cpass_bias_reg[4];
385+
cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
386+
cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
387+
cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
388+
cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
389+
result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Y, 4, (const unsigned char*)&cpass_bias_reg);
390+
return result;
391+
}
392+
393+
ICM_20948_Status_e ICM_20948::SetBiasCPassZ( int32_t newValue)
394+
{
395+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
396+
unsigned char cpass_bias_reg[4];
397+
cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
398+
cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
399+
cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
400+
cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
401+
result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Z, 4, (const unsigned char*)&cpass_bias_reg);
402+
return result;
403+
}
404+
405+
ICM_20948_Status_e ICM_20948::GetBiasCPassX( int32_t* bias)
406+
{
407+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
408+
unsigned char bias_data[4] = { 0 };
409+
result = inv_icm20948_read_mems(&_device, CPASS_BIAS_X, 4, bias_data);
410+
bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
411+
return result;
412+
}
413+
414+
ICM_20948_Status_e ICM_20948::GetBiasCPassY( int32_t* bias)
415+
{
416+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
417+
unsigned char bias_data[4] = { 0 };
418+
result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Y, 4, bias_data);
419+
bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
420+
return result;
421+
}
422+
423+
ICM_20948_Status_e ICM_20948::GetBiasCPassZ( int32_t* bias)
424+
{
425+
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
426+
unsigned char bias_data[4] = { 0 };
427+
result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Z, 4, bias_data);
428+
bias[0] = (int32_t)(bias_data[0] << 24) | (bias_data[1] << 16) | (bias_data[2] << 8) | (bias_data[3]);
429+
return result;
430+
}
431+
242432
float ICM_20948::temp(void)
243433
{
244434
return getTempC(agmt.tmp.val);
@@ -1796,3 +1986,5 @@ ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len,
17961986

17971987
return ICM_20948_Stat_Ok;
17981988
}
1989+
1990+

‎src/ICM_20948.h

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -173,6 +173,28 @@ class ICM_20948
173173

174174
//DMP
175175

176+
//Gyro Bias
177+
ICM_20948_Status_e SetBiasGyroX( int32_t newValue);
178+
ICM_20948_Status_e SetBiasGyroY( int32_t newValue);
179+
ICM_20948_Status_e SetBiasGyroZ( int32_t newValue);
180+
ICM_20948_Status_e GetBiasGyroX( int32_t* bias);
181+
ICM_20948_Status_e GetBiasGyroY( int32_t* bias);
182+
ICM_20948_Status_e GetBiasGyroZ( int32_t* bias);
183+
//Accel Bias
184+
ICM_20948_Status_e SetBiasAccelX( int32_t newValue);
185+
ICM_20948_Status_e SetBiasAccelY( int32_t newValue);
186+
ICM_20948_Status_e SetBiasAccelZ( int32_t newValue);
187+
ICM_20948_Status_e GetBiasAccelX( int32_t* bias);
188+
ICM_20948_Status_e GetBiasAccelY( int32_t* bias);
189+
ICM_20948_Status_e GetBiasAccelZ( int32_t* bias);
190+
//CPass Bias
191+
ICM_20948_Status_e SetBiasCPassX( int32_t newValue);
192+
ICM_20948_Status_e SetBiasCPassY( int32_t newValue);
193+
ICM_20948_Status_e SetBiasCPassZ( int32_t newValue);
194+
ICM_20948_Status_e GetBiasCPassX( int32_t* bias);
195+
ICM_20948_Status_e GetBiasCPassY( int32_t* bias);
196+
ICM_20948_Status_e GetBiasCPassZ( int32_t* bias);
197+
176198
// Done:
177199
// Configure DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
178200
// Load Firmware

‎src/util/ICM_20948_C.c

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010

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

25942594
return result;
25952595
}
2596+
2597+

0 commit comments

Comments
 (0)