Skip to content

Commit 74ec8f0

Browse files
PaulZCPaulZC
committed
Check DMP is supported. Update status.
1 parent 32c78ee commit 74ec8f0

File tree

1 file changed

+108
-54
lines changed

1 file changed

+108
-54
lines changed

‎src/ICM_20948.cpp

Lines changed: 108 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -242,236 +242,290 @@ float ICM_20948::getGyrDPS(int16_t axis_val)
242242
//Gyro Bias
243243
ICM_20948_Status_e ICM_20948::setBiasGyroX( int32_t newValue)
244244
{
245-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
245+
if (_device._dmp_firmware_available == true) // Is DMP supported?
246+
{
246247
unsigned char gyro_bias_reg[4];
247248
gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
248249
gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
249250
gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
250251
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;
252+
status = inv_icm20948_write_mems(&_device, GYRO_BIAS_X, 4, (const unsigned char*)&gyro_bias_reg);
253+
return status;
254+
}
255+
return ICM_20948_Stat_DMPNotSupported;
253256
}
254257

255258
ICM_20948_Status_e ICM_20948::setBiasGyroY( int32_t newValue)
256259
{
257-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
260+
if (_device._dmp_firmware_available == true) // Is DMP supported?
261+
{
258262
unsigned char gyro_bias_reg[4];
259263
gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
260264
gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
261265
gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
262266
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;
267+
status = inv_icm20948_write_mems(&_device, GYRO_BIAS_Y, 4, (const unsigned char*)&gyro_bias_reg);
268+
return status;
269+
}
270+
return ICM_20948_Stat_DMPNotSupported;
265271
}
266272

267273
ICM_20948_Status_e ICM_20948::setBiasGyroZ( int32_t newValue)
268274
{
269-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
275+
if (_device._dmp_firmware_available == true) // Is DMP supported?
276+
{
270277
unsigned char gyro_bias_reg[4];
271278
gyro_bias_reg[0] = (unsigned char)(newValue >> 24);
272279
gyro_bias_reg[1] = (unsigned char)(newValue >> 16);
273280
gyro_bias_reg[2] = (unsigned char)(newValue >> 8);
274281
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;
282+
status = inv_icm20948_write_mems(&_device, GYRO_BIAS_Z, 4, (const unsigned char*)&gyro_bias_reg);
283+
return status;
284+
}
285+
return ICM_20948_Stat_DMPNotSupported;
277286
}
278287

279288
ICM_20948_Status_e ICM_20948::getBiasGyroX( int32_t* bias)
280289
{
281-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
290+
if (_device._dmp_firmware_available == true) // Is DMP supported?
291+
{
282292
unsigned char bias_data[4] = { 0 };
283-
result = inv_icm20948_read_mems(&_device, GYRO_BIAS_X, 4, bias_data);
293+
status = inv_icm20948_read_mems(&_device, GYRO_BIAS_X, 4, bias_data);
284294
union {
285295
int32_t signed32;
286296
uint32_t unsigned32;
287297
} signedUnsigned32;
288298
signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
289299
*bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
290-
return result;
300+
return status;
301+
}
302+
return ICM_20948_Stat_DMPNotSupported;
291303
}
292304

293305
ICM_20948_Status_e ICM_20948::getBiasGyroY( int32_t* bias)
294306
{
295-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
307+
if (_device._dmp_firmware_available == true) // Is DMP supported?
308+
{
296309
unsigned char bias_data[4] = { 0 };
297-
result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Y, 4, bias_data);
310+
status = inv_icm20948_read_mems(&_device, GYRO_BIAS_Y, 4, bias_data);
298311
union {
299312
int32_t signed32;
300313
uint32_t unsigned32;
301314
} signedUnsigned32;
302315
signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
303316
*bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
304-
return result;
317+
return status;
318+
}
319+
return ICM_20948_Stat_DMPNotSupported;
305320
}
306321

307322
ICM_20948_Status_e ICM_20948::getBiasGyroZ( int32_t* bias)
308323
{
309-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
324+
if (_device._dmp_firmware_available == true) // Is DMP supported?
325+
{
310326
unsigned char bias_data[4] = { 0 };
311-
result = inv_icm20948_read_mems(&_device, GYRO_BIAS_Z, 4, bias_data);
327+
status = inv_icm20948_read_mems(&_device, GYRO_BIAS_Z, 4, bias_data);
312328
union {
313329
int32_t signed32;
314330
uint32_t unsigned32;
315331
} signedUnsigned32;
316332
signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
317333
*bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
318-
return result;
334+
return status;
335+
}
336+
return ICM_20948_Stat_DMPNotSupported;
319337
}
320338
//Accel Bias
321339
ICM_20948_Status_e ICM_20948::setBiasAccelX( int32_t newValue)
322340
{
323-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
341+
if (_device._dmp_firmware_available == true) // Is DMP supported?
342+
{
324343
unsigned char accel_bias_reg[4];
325344
accel_bias_reg[0] = (unsigned char)(newValue >> 24);
326345
accel_bias_reg[1] = (unsigned char)(newValue >> 16);
327346
accel_bias_reg[2] = (unsigned char)(newValue >> 8);
328347
accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
329-
result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_X, 4, (const unsigned char*)&accel_bias_reg);
330-
return result;
348+
status = inv_icm20948_write_mems(&_device, ACCEL_BIAS_X, 4, (const unsigned char*)&accel_bias_reg);
349+
return status;
350+
}
351+
return ICM_20948_Stat_DMPNotSupported;
331352
}
332353

333354
ICM_20948_Status_e ICM_20948::setBiasAccelY( int32_t newValue)
334355
{
335-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
356+
if (_device._dmp_firmware_available == true) // Is DMP supported?
357+
{
336358
unsigned char accel_bias_reg[4];
337359
accel_bias_reg[0] = (unsigned char)(newValue >> 24);
338360
accel_bias_reg[1] = (unsigned char)(newValue >> 16);
339361
accel_bias_reg[2] = (unsigned char)(newValue >> 8);
340362
accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
341-
result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Y, 4, (const unsigned char*)&accel_bias_reg);
342-
return result;
363+
status = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Y, 4, (const unsigned char*)&accel_bias_reg);
364+
return status;
365+
}
366+
return ICM_20948_Stat_DMPNotSupported;
343367
}
344368

345369
ICM_20948_Status_e ICM_20948::setBiasAccelZ( int32_t newValue)
346370
{
347-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
371+
if (_device._dmp_firmware_available == true) // Is DMP supported?
372+
{
348373
unsigned char accel_bias_reg[4];
349374
accel_bias_reg[0] = (unsigned char)(newValue >> 24);
350375
accel_bias_reg[1] = (unsigned char)(newValue >> 16);
351376
accel_bias_reg[2] = (unsigned char)(newValue >> 8);
352377
accel_bias_reg[3] = (unsigned char)(newValue & 0xff);
353-
result = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Z, 4, (const unsigned char*)&accel_bias_reg);
354-
return result;
378+
status = inv_icm20948_write_mems(&_device, ACCEL_BIAS_Z, 4, (const unsigned char*)&accel_bias_reg);
379+
return status;
380+
}
381+
return ICM_20948_Stat_DMPNotSupported;
355382
}
356383

357384
ICM_20948_Status_e ICM_20948::getBiasAccelX( int32_t* bias)
358385
{
359-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
386+
if (_device._dmp_firmware_available == true) // Is DMP supported?
387+
{
360388
unsigned char bias_data[4] = { 0 };
361-
result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_X, 4, bias_data);
389+
status = inv_icm20948_read_mems(&_device, ACCEL_BIAS_X, 4, bias_data);
362390
union {
363391
int32_t signed32;
364392
uint32_t unsigned32;
365393
} signedUnsigned32;
366394
signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
367395
*bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
368-
return result;
396+
return status;
397+
}
398+
return ICM_20948_Stat_DMPNotSupported;
369399
}
370400

371401
ICM_20948_Status_e ICM_20948::getBiasAccelY( int32_t* bias)
372402
{
373-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
403+
if (_device._dmp_firmware_available == true) // Is DMP supported?
404+
{
374405
unsigned char bias_data[4] = { 0 };
375-
result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Y, 4, bias_data);
406+
status = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Y, 4, bias_data);
376407
union {
377408
int32_t signed32;
378409
uint32_t unsigned32;
379410
} signedUnsigned32;
380411
signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
381412
*bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
382-
return result;
413+
return status;
414+
}
415+
return ICM_20948_Stat_DMPNotSupported;
383416
}
384417

385418
ICM_20948_Status_e ICM_20948::getBiasAccelZ( int32_t* bias)
386419
{
387-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
420+
if (_device._dmp_firmware_available == true) // Is DMP supported?
421+
{
388422
unsigned char bias_data[4] = { 0 };
389-
result = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Z, 4, bias_data);
423+
status = inv_icm20948_read_mems(&_device, ACCEL_BIAS_Z, 4, bias_data);
390424
union {
391425
int32_t signed32;
392426
uint32_t unsigned32;
393427
} signedUnsigned32;
394428
signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
395429
*bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
396-
return result;
430+
return status;
431+
}
432+
return ICM_20948_Stat_DMPNotSupported;
397433
}
398434
//CPass Bias
399435
ICM_20948_Status_e ICM_20948::setBiasCPassX( int32_t newValue)
400436
{
401-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
437+
if (_device._dmp_firmware_available == true) // Is DMP supported?
438+
{
402439
unsigned char cpass_bias_reg[4];
403440
cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
404441
cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
405442
cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
406443
cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
407-
result = inv_icm20948_write_mems(&_device, CPASS_BIAS_X, 4, (const unsigned char*)&cpass_bias_reg);
408-
return result;
444+
status = inv_icm20948_write_mems(&_device, CPASS_BIAS_X, 4, (const unsigned char*)&cpass_bias_reg);
445+
return status;
446+
}
447+
return ICM_20948_Stat_DMPNotSupported;
409448
}
410449

411450
ICM_20948_Status_e ICM_20948::setBiasCPassY( int32_t newValue)
412451
{
413-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
452+
if (_device._dmp_firmware_available == true) // Is DMP supported?
453+
{
414454
unsigned char cpass_bias_reg[4];
415455
cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
416456
cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
417457
cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
418458
cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
419-
result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Y, 4, (const unsigned char*)&cpass_bias_reg);
420-
return result;
459+
status = inv_icm20948_write_mems(&_device, CPASS_BIAS_Y, 4, (const unsigned char*)&cpass_bias_reg);
460+
return status;
461+
}
462+
return ICM_20948_Stat_DMPNotSupported;
421463
}
422464

423465
ICM_20948_Status_e ICM_20948::setBiasCPassZ( int32_t newValue)
424466
{
425-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
467+
if (_device._dmp_firmware_available == true) // Is DMP supported?
468+
{
426469
unsigned char cpass_bias_reg[4];
427470
cpass_bias_reg[0] = (unsigned char)(newValue >> 24);
428471
cpass_bias_reg[1] = (unsigned char)(newValue >> 16);
429472
cpass_bias_reg[2] = (unsigned char)(newValue >> 8);
430473
cpass_bias_reg[3] = (unsigned char)(newValue & 0xff);
431-
result = inv_icm20948_write_mems(&_device, CPASS_BIAS_Z, 4, (const unsigned char*)&cpass_bias_reg);
432-
return result;
474+
status = inv_icm20948_write_mems(&_device, CPASS_BIAS_Z, 4, (const unsigned char*)&cpass_bias_reg);
475+
return status;
476+
}
477+
return ICM_20948_Stat_DMPNotSupported;
433478
}
434479

435480
ICM_20948_Status_e ICM_20948::getBiasCPassX( int32_t* bias)
436481
{
437-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
482+
if (_device._dmp_firmware_available == true) // Is DMP supported?
483+
{
438484
unsigned char bias_data[4] = { 0 };
439-
result = inv_icm20948_read_mems(&_device, CPASS_BIAS_X, 4, bias_data);
485+
status = inv_icm20948_read_mems(&_device, CPASS_BIAS_X, 4, bias_data);
440486
union {
441487
int32_t signed32;
442488
uint32_t unsigned32;
443489
} signedUnsigned32;
444490
signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
445491
*bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
446-
return result;
492+
return status;
493+
}
494+
return ICM_20948_Stat_DMPNotSupported;
447495
}
448496

449497
ICM_20948_Status_e ICM_20948::getBiasCPassY( int32_t* bias)
450498
{
451-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
499+
if (_device._dmp_firmware_available == true) // Is DMP supported?
500+
{
452501
unsigned char bias_data[4] = { 0 };
453-
result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Y, 4, bias_data);
502+
status = inv_icm20948_read_mems(&_device, CPASS_BIAS_Y, 4, bias_data);
454503
union {
455504
int32_t signed32;
456505
uint32_t unsigned32;
457506
} signedUnsigned32;
458507
signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
459508
*bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
460-
return result;
509+
return status;
510+
}
511+
return ICM_20948_Stat_DMPNotSupported;
461512
}
462513

463514
ICM_20948_Status_e ICM_20948::getBiasCPassZ( int32_t* bias)
464515
{
465-
ICM_20948_Status_e result = ICM_20948_Stat_Ok;
516+
if (_device._dmp_firmware_available == true) // Is DMP supported?
517+
{
466518
unsigned char bias_data[4] = { 0 };
467-
result = inv_icm20948_read_mems(&_device, CPASS_BIAS_Z, 4, bias_data);
519+
status = inv_icm20948_read_mems(&_device, CPASS_BIAS_Z, 4, bias_data);
468520
union {
469521
int32_t signed32;
470522
uint32_t unsigned32;
471523
} signedUnsigned32;
472524
signedUnsigned32.unsigned32 = (((uint32_t)bias_data[0]) << 24) | (((uint32_t)bias_data[1]) << 16) | (((uint32_t)bias_data[2]) << 8) | (bias_data[3]);
473525
*bias = signedUnsigned32.signed32; // Convert from unsigned to signed with no cast ambiguity
474-
return result;
526+
return status;
527+
}
528+
return ICM_20948_Stat_DMPNotSupported;
475529
}
476530

477531
float ICM_20948::temp(void)

0 commit comments

Comments
 (0)