Skip to content

Commit c15b926

Browse files
CallumMackenzieCallumMackenzie
committed
Merge imu_subsystems
2 parents d24ae5d + 82b1cde commit c15b926

File tree

2 files changed

+135
-18
lines changed

2 files changed

+135
-18
lines changed

‎main/imu/imu_icm20948_driver.c‎

Lines changed: 134 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -302,7 +302,7 @@ biodyn_imu_err_t biodyn_imu_icm20948_init()
302302
if (err != ESP_OK)
303303
{
304304
biodyn_imu_icm20948_add_error_to_subsystem(err, "ICM_20948_INIT: error in completing self-test. Further investigation into self-test part functions likely required.");
305-
ESP_LOGE(TAG, "Failed to initialize and start AK09916 (magnetometer) device, error %d", err);
305+
ESP_LOGE(TAG, "Failed self_test for icm20948, investiage into composite function parts of self_test, error: %d", err);
306306
return BIODYN_IMU_ERR_COULDNT_INIT_SPI_DEV;
307307
}
308308
// TODO: write self_tests for magnetometer
@@ -622,6 +622,107 @@ static biodyn_imu_err_t self_test_accel_gyro()
622622
deltas.gyro_z = imd_st_on.gyro_z - imd_st_off.gyro_z;
623623

624624
// Obtain factory self_test results from icm20948 self_test registers to compare to.
625+
uint8_t trimax = 0;
626+
uint8_t trimay = 0;
627+
uint8_t trimaz = 0;
628+
uint8_t trimgx = 0;
629+
uint8_t trimgy = 0;
630+
uint8_t trimgz = 0;
631+
632+
biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_X_ACCEL, &trimax);
633+
biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_Y_ACCEL, &trimay);
634+
biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_Z_ACCEL, &trimaz);
635+
biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_X_GYRO, &trimgx);
636+
biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_Y_GYRO, &trimgy);
637+
biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_Z_GYRO, &trimgz);
638+
639+
// Compare trims to with a range limiter to deltas
640+
// Compare within magnitude by 50% range (might need to be adjusted)
641+
float RL = 0.5;
642+
float RH = 1.5;
643+
/** DEBUG*/
644+
ESP_LOGI(TAG, "\n st_off ax: %f\n st_off ay: %f\n st_off az: %f\n st_off gx: %f\n st_off gy: %f\n st_off gz: %f", imd_st_off.accel_x, imd_st_off.accel_y, imd_st_off.accel_z, imd_st_off.gyro_x, imd_st_off.gyro_y, imd_st_off.gyro_z);
645+
ESP_LOGI(TAG, "\n st_on ax: %f\n st_on ay: %f\n st_on az: %f\n st_on gx: %f\n st_on gy: %f\n st_on gz: %f", imd_st_on.accel_x, imd_st_on.accel_y, imd_st_on.accel_z, imd_st_on.gyro_x, imd_st_on.gyro_y, imd_st_on.gyro_z);
646+
647+
ESP_LOGI(TAG, "\n delta ax: %f\n delta ay: %f\n delta az: %f\n delta gx: %f\n delta gy: %f\n delta gz: %f", deltas.accel_x, deltas.accel_y, deltas.accel_z, deltas.gyro_x, deltas.gyro_y, deltas.gyro_z);
648+
ESP_LOGI(TAG, "\n trimax: %d\ntrimay: %d\n trimaz: %d\n trimgx: %d\n trimgy: %d\n trimgz: %d", trimax, trimay, trimaz, trimgx, trimgy, trimgz);
649+
/** DEBUG */
650+
651+
// CHECK if deltas-trim is between rangel*trim and rangeh*trim
652+
// return error on fails
653+
ESP_LOGI(TAG, "checking against trims");
654+
if (RL * trimax < deltas.accel_x / (1 << accel_range_value) && RH * trimax > deltas.accel_x / (1 << accel_range_value))
655+
{
656+
ESP_LOGI(TAG, "Self-test succesful for accel_gyro on ax");
657+
}
658+
else
659+
{
660+
ESP_LOGE(TAG, "self test failed on delta ax with ax = %f and trimax = %d", deltas.accel_x, trimax);
661+
biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on ax");
662+
return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
663+
}
664+
if (RL * trimay < deltas.accel_y / (1 << accel_range_value) && RH * trimay > deltas.accel_y / (1 << accel_range_value))
665+
{
666+
ESP_LOGI(TAG, "Self-test succesful for accel_gyro on ay");
667+
}
668+
else
669+
{
670+
ESP_LOGE(TAG, "self test failed on delta ay");
671+
672+
biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on ay");
673+
return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
674+
}
675+
if (RL * trimaz < deltas.accel_z / (1 << accel_range_value) && RH * trimaz > deltas.accel_z / (1 << accel_range_value))
676+
{
677+
ESP_LOGI(TAG, "Self-test succesful for accel_gyro on az");
678+
}
679+
else
680+
{
681+
ESP_LOGE(TAG, "self test failed on delta az");
682+
biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on az");
683+
return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
684+
}
685+
if (RL * trimgx < deltas.gyro_x / (1 << gyro_range_value) && RH * trimgx > deltas.gyro_x / (1 << gyro_range_value))
686+
{
687+
ESP_LOGI(TAG, "Self-test succesful for accel_gyro on gx");
688+
}
689+
else
690+
{
691+
uint8_t and1 = RL * trimgx < deltas.gyro_x / (1 << gyro_range_value);
692+
uint8_t and2 = RH * trimgx > deltas.gyro_x / (1 << gyro_range_value);
693+
ESP_LOGE(TAG, "failed with delta_gx = %f and trimgx = %d", deltas.gyro_x, trimgx);
694+
ESP_LOGE(TAG, "and1 = %d; and2 = %d", and1, and2);
695+
biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on gx");
696+
return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
697+
}
698+
if (RL * trimgy < deltas.gyro_y / (1 << gyro_range_value) && RH * trimgy > deltas.gyro_y / (1 << gyro_range_value))
699+
{
700+
ESP_LOGI(TAG, "Self-test succesful for accel_gyro on gy");
701+
}
702+
else
703+
{
704+
uint8_t and1 = RL * trimgy < deltas.gyro_y / (1 << gyro_range_value);
705+
uint8_t and2 = RH * trimgy > deltas.gyro_y / (1 << gyro_range_value);
706+
ESP_LOGE(TAG, "failed with delta_gx = %f and trimgx = %d", deltas.gyro_y, trimgy);
707+
ESP_LOGE(TAG, "and1 = %d; and2 = %d", and1, and2);
708+
biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on gy");
709+
return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
710+
}
711+
if (RL * trimgz < deltas.gyro_z / (1 << gyro_range_value) && RH * trimgz > deltas.gyro_z / (1 << gyro_range_value))
712+
{
713+
ESP_LOGI(TAG, "Self-test succesful for accel_gyro on gz");
714+
}
715+
else
716+
{
717+
uint8_t and1 = RL * trimgz < deltas.gyro_z / (1 << gyro_range_value);
718+
uint8_t and2 = RH * trimgz > deltas.gyro_z / (1 << gyro_range_value);
719+
ESP_LOGE(TAG, "failed with delta_gx = %f and trimgx = %d", deltas.gyro_z, trimgy);
720+
ESP_LOGE(TAG, "and1 = %d; and2 = %d", and1, and2);
721+
722+
biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on gz");
723+
return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
724+
}
725+
ESP_LOGI(TAG, "Finished checking against trims");
625726

626727
// Clear self_test bits
627728
// ACCEL SELF_TEST END
@@ -641,8 +742,8 @@ static biodyn_imu_err_t self_test_accel_gyro()
641742
return BIODYN_IMU_OK;
642743
}
643744

644-
// TODO: also check gyro self-test
645-
static biodyn_imu_err_t self_test_accel()
745+
// TESTING accel_gyro replacement
746+
/*static biodyn_imu_err_t self_test_accel()
646747
{
647748
// SELF-TEST RESPONSE = SENSOR OUTPUT WITH SELF-TEST ENABLED – SENSOR OUTPUT WITHOUT SELF-TEST ENABLED (p. 24)
648749
// Sensor output without self_test_enabled
@@ -660,11 +761,7 @@ static biodyn_imu_err_t self_test_accel()
660761
return BIODYN_IMU_OK;
661762
}
662763
663-
// TODO: Fix this self-test
664-
/**
665-
* Self-test the gyroscope of the IMU.
666-
* Checks the built-in self-test in the IMU, which identifies if it's gyroscope is working.
667-
*/
764+
668765
static biodyn_imu_err_t self_test_gyro()
669766
{
670767
biodyn_imu_err_t err;
@@ -691,6 +788,7 @@ static biodyn_imu_err_t self_test_gyro()
691788
692789
return BIODYN_IMU_OK;
693790
}
791+
*/
694792

695793
/**
696794
* Self-test the magnetometer of the IMU.
@@ -717,6 +815,7 @@ static biodyn_imu_err_t self_test_mag()
717815
biodyn_imu_icm20948_read_reg(_b0, EXT_SLV_SENS_DATA_00, &temp);
718816
if (temp & 0b11111111)
719817
return BIODYN_IMU_OK;
818+
ESP_LOGE(TAG, "SELF_TEST_MAG: Failed self-test with read value test actual: %x, expected: non-zero", temp);
720819
biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_MAG: Failed self-test");
721820
return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
722821
}
@@ -737,28 +836,45 @@ biodyn_imu_err_t biodyn_imu_icm20948_self_test()
737836
ESP_LOGE(TAG, "Self test failed - whoami (%x)", err);
738837
return err;
739838
}
839+
ESP_LOGI(TAG, "Correct WHOAMI");
840+
740841
// Check that user bank selection works
741842
if ((err = self_test_user_banks()))
742843
{
743844
ESP_LOGE(TAG, "Self test failed - user banks (%x)", err);
744845
return err;
745846
}
746-
// Run gyro self-test
747-
if ((err = self_test_gyro()))
748-
{
749-
ESP_LOGE(TAG, "Self test failed - gyro (%x)", err);
750-
}
751-
// Run accel self-test
752-
if ((err = self_test_accel()))
753-
{
754-
ESP_LOGE(TAG, "Self test failed - accel (%x)", err);
755-
}
847+
// // Run gyro self-test
848+
// if ((err = self_test_gyro()))
849+
// {
850+
// ESP_LOGE(TAG, "Self test failed - gyro (%x)", err);
851+
// }
852+
// // Run accel self-test
853+
// if ((err = self_test_accel()))
854+
// {
855+
// ESP_LOGE(TAG, "Self test failed - accel (%x)", err);
856+
// }
857+
858+
// Run accel and gyro self-test
859+
// TODO: REPAIR
860+
// if ((err = self_test_accel_gyro()))
861+
// {
862+
// ESP_LOGE(TAG, "Self test failed - accel and gyro (%x)", err);
863+
// }
756864

757865
// Run mag self-test
866+
<<<<<<< HEAD
758867
// if ((err = self_test_mag()))
759868
// {
760869
// ESP_LOGE(TAG, "Self test failed - mag (%x)", err);
761870
// }
871+
=======
872+
if ((err = self_test_mag()))
873+
{
874+
ESP_LOGE(TAG, "Self test failed - mag (%x)", err);
875+
return err;
876+
}
877+
>>>>>>> imu_subsystems
762878

763879
return BIODYN_IMU_OK;
764880
}

‎main/imu/imu_icm20948_driver.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -191,5 +191,6 @@ biodyn_imu_err_t biodyn_imu_icm20948_config_gyro_sample_averaging(uint8_t gyro_s
191191
biodyn_imu_err_t biodyn_imu_icm20948_config_gyro_sample_averaging(uint8_t gyro_smplrt_div);
192192

193193
// TODO rest of functions for configuring IMU
194+
// TODO remove bias from gyro and mag
194195

195196
#endif // ICM20948_DRIVER_H

0 commit comments

Comments
 (0)