Merge branch 'datafast'

Author: CallumMackenzie

main/CMakeLists.txt

@@ -6,7 +6,7 @@ idf_component_register(
     "system/led.c"
     "system/self_test.c"
     "system/time_sync.c"
-    "imu/data_fast.c"
+    "system/data_fast.c"
     "main.c"
     INCLUDE_DIRS "."
 )

main/biodyn_systems.h

@@ -5,7 +5,7 @@
 #include "system/time_sync.h"
 #include "system/led.h"
 #include "imu/imu_icm20948_driver.h"
-#include "imu/data_fast.h"
+#include "system/data_fast.h"
 
 // Order is order of initialization
 const static biodyn_system biodyn_systems[] = {

main/bluetooth/data_fast_service.h

@@ -3,24 +3,17 @@
 
 #include "constants.h"
 #include "bluetooth/ble.h"
-#include "imu/data_fast.h"
+#include "system/data_fast.h"
 #include "system/time_sync.h"
 
 const static struct biodyn_ble_characteristic data_fast_chars[] = {
-	{
-		.name = "Packed IMU Data",
-		.uuid = BIODYN_BLE_UUID_16(0x4153),
-		.permissions = BIODYN_PERM_READ,
-		.properties = BIODYN_PROP_READ,
-		.get_data = ble_data_fast_packed_imu,
-	},
 	{
 		.name = "Packed Collective Data",
-		.uuid = BIODYN_BLE_UUID_16(0x4155),
+		.uuid = BIODYN_BLE_UUID_16(0x4153),
 		.permissions = BIODYN_PERM_READ,
 		.properties = BIODYN_PROP_READ,
 		// TODO: Implement packed collective data read
-		// .get_data = ,
+		.get_data = ble_data_fast_packed,
 	},
 	{
 		.name = "Heartbeat",

main/constants.h

@@ -11,7 +11,7 @@
 #define BIODYN_HARDWARE_VERSION "v0"
 #define BIODYN_MODEL_NUMBER "0001"
 #define BIODYN_SERIAL_NUMBER "000T"
-#define BIODYN_FIRMWARE_VERSION "0.0.3"
+#define BIODYN_FIRMWARE_VERSION "0.0.4"
 #define BIODYN_SYSTEM_ID "Alpha"
 
 // LOG TAGS

main/imu/data_fast.c

@@ -303,7 +303,7 @@ biodyn_imu_err_t biodyn_imu_icm20948_init()
 	if (err != ESP_OK)
 	{
 		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.");
-		ESP_LOGE(TAG, "Failed to initialize and start AK09916 (magnetometer) device, error %d", err);
+		ESP_LOGE(TAG, "Failed self_test for icm20948, investiage into composite function parts of self_test, error: %d", err);
 		return BIODYN_IMU_ERR_COULDNT_INIT_SPI_DEV;
 	}
 	// TODO: write self_tests for magnetometer
@@ -623,6 +623,107 @@ static biodyn_imu_err_t self_test_accel_gyro()
 	deltas.gyro_z = imd_st_on.gyro_z - imd_st_off.gyro_z;
 
 	// Obtain factory self_test results from icm20948 self_test registers to compare to.
+	uint8_t trimax = 0;
+	uint8_t trimay = 0;
+	uint8_t trimaz = 0;
+	uint8_t trimgx = 0;
+	uint8_t trimgy = 0;
+	uint8_t trimgz = 0;
+
+	biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_X_ACCEL, &trimax);
+	biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_Y_ACCEL, &trimay);
+	biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_Z_ACCEL, &trimaz);
+	biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_X_GYRO, &trimgx);
+	biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_Y_GYRO, &trimgy);
+	biodyn_imu_icm20948_read_reg(_b1, SELF_TEST_Z_GYRO, &trimgz);
+
+	// Compare trims to with a range limiter to deltas
+	// Compare within magnitude by 50% range (might need to be adjusted)
+	float RL = 0.5;
+	float RH = 1.5;
+	/** DEBUG*/
+	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);
+	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);
+
+	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);
+	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);
+	/** DEBUG */
+
+	// CHECK if deltas-trim is between rangel*trim and rangeh*trim
+	// return error on fails
+	ESP_LOGI(TAG, "checking against trims");
+	if (RL * trimax < deltas.accel_x / (1 << accel_range_value) && RH * trimax > deltas.accel_x / (1 << accel_range_value))
+	{
+		ESP_LOGI(TAG, "Self-test succesful for accel_gyro on ax");
+	}
+	else
+	{
+		ESP_LOGE(TAG, "self test failed on delta ax with ax = %f and trimax = %d", deltas.accel_x, trimax);
+		biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on ax");
+		return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
+	}
+	if (RL * trimay < deltas.accel_y / (1 << accel_range_value) && RH * trimay > deltas.accel_y / (1 << accel_range_value))
+	{
+		ESP_LOGI(TAG, "Self-test succesful for accel_gyro on ay");
+	}
+	else
+	{
+		ESP_LOGE(TAG, "self test failed on delta ay");
+
+		biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on ay");
+		return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
+	}
+	if (RL * trimaz < deltas.accel_z / (1 << accel_range_value) && RH * trimaz > deltas.accel_z / (1 << accel_range_value))
+	{
+		ESP_LOGI(TAG, "Self-test succesful for accel_gyro on az");
+	}
+	else
+	{
+		ESP_LOGE(TAG, "self test failed on delta az");
+		biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on az");
+		return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
+	}
+	if (RL * trimgx < deltas.gyro_x / (1 << gyro_range_value) && RH * trimgx > deltas.gyro_x / (1 << gyro_range_value))
+	{
+		ESP_LOGI(TAG, "Self-test succesful for accel_gyro on gx");
+	}
+	else
+	{
+		uint8_t and1 = RL * trimgx < deltas.gyro_x / (1 << gyro_range_value);
+		uint8_t and2 = RH * trimgx > deltas.gyro_x / (1 << gyro_range_value);
+		ESP_LOGE(TAG, "failed with delta_gx = %f and trimgx = %d", deltas.gyro_x, trimgx);
+		ESP_LOGE(TAG, "and1 = %d; and2 = %d", and1, and2);
+		biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on gx");
+		return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
+	}
+	if (RL * trimgy < deltas.gyro_y / (1 << gyro_range_value) && RH * trimgy > deltas.gyro_y / (1 << gyro_range_value))
+	{
+		ESP_LOGI(TAG, "Self-test succesful for accel_gyro on gy");
+	}
+	else
+	{
+		uint8_t and1 = RL * trimgy < deltas.gyro_y / (1 << gyro_range_value);
+		uint8_t and2 = RH * trimgy > deltas.gyro_y / (1 << gyro_range_value);
+		ESP_LOGE(TAG, "failed with delta_gx = %f and trimgx = %d", deltas.gyro_y, trimgy);
+		ESP_LOGE(TAG, "and1 = %d; and2 = %d", and1, and2);
+		biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on gy");
+		return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
+	}
+	if (RL * trimgz < deltas.gyro_z / (1 << gyro_range_value) && RH * trimgz > deltas.gyro_z / (1 << gyro_range_value))
+	{
+		ESP_LOGI(TAG, "Self-test succesful for accel_gyro on gz");
+	}
+	else
+	{
+		uint8_t and1 = RL * trimgz < deltas.gyro_z / (1 << gyro_range_value);
+		uint8_t and2 = RH * trimgz > deltas.gyro_z / (1 << gyro_range_value);
+		ESP_LOGE(TAG, "failed with delta_gx = %f and trimgx = %d", deltas.gyro_z, trimgy);
+		ESP_LOGE(TAG, "and1 = %d; and2 = %d", and1, and2);
+
+		biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_ACCEL_GYRO: failed self-test on gz");
+		return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
+	}
+	ESP_LOGI(TAG, "Finished checking against trims");
 
 	// Clear self_test bits
 	// ACCEL SELF_TEST END
@@ -642,8 +743,8 @@ static biodyn_imu_err_t self_test_accel_gyro()
 	return BIODYN_IMU_OK;
 }
 
-// TODO: also check gyro self-test
-static biodyn_imu_err_t self_test_accel()
+// TESTING accel_gyro replacement
+/*static biodyn_imu_err_t self_test_accel()
 {
 	// SELF-TEST RESPONSE = SENSOR OUTPUT WITH SELF-TEST ENABLED – SENSOR OUTPUT WITHOUT SELF-TEST ENABLED (p. 24)
 	// Sensor output without self_test_enabled
@@ -661,11 +762,7 @@ static biodyn_imu_err_t self_test_accel()
 	return BIODYN_IMU_OK;
 }
 
-// TODO: Fix this self-test
-/**
- * Self-test the gyroscope of the IMU.
- * Checks the built-in self-test in the IMU, which identifies if it's gyroscope is working.
- */
+
 static biodyn_imu_err_t self_test_gyro()
 {
 	biodyn_imu_err_t err;
@@ -692,6 +789,7 @@ static biodyn_imu_err_t self_test_gyro()
 
 	return BIODYN_IMU_OK;
 }
+*/
 
 /**
  * Self-test the magnetometer of the IMU.
@@ -718,6 +816,7 @@ static biodyn_imu_err_t self_test_mag()
 	biodyn_imu_icm20948_read_reg(_b0, EXT_SLV_SENS_DATA_00, &temp);
 	if (temp & 0b11111111)
 		return BIODYN_IMU_OK;
+	ESP_LOGE(TAG, "SELF_TEST_MAG: Failed self-test with read value test actual: %x, expected: non-zero", temp);
 	biodyn_imu_icm20948_add_error_to_subsystem(BIODYN_IMU_ERR_COULDNT_CONFIGURE, "SELF_TEST_MAG: Failed self-test");
 	return BIODYN_IMU_ERR_COULDNT_CONFIGURE;
 }
@@ -738,28 +837,45 @@ biodyn_imu_err_t biodyn_imu_icm20948_self_test()
 		ESP_LOGE(TAG, "Self test failed - whoami (%x)", err);
 		return err;
 	}
+	ESP_LOGI(TAG, "Correct WHOAMI");
+
 	// Check that user bank selection works
 	if ((err = self_test_user_banks()))
 	{
 		ESP_LOGE(TAG, "Self test failed - user banks (%x)", err);
 		return err;
 	}
-	// Run gyro self-test
-	if ((err = self_test_gyro()))
-	{
-		ESP_LOGE(TAG, "Self test failed - gyro (%x)", err);
-	}
-	// Run accel self-test
-	if ((err = self_test_accel()))
-	{
-		ESP_LOGE(TAG, "Self test failed - accel (%x)", err);
-	}
+	// // Run gyro self-test
+	// if ((err = self_test_gyro()))
+	// {
+	// 	ESP_LOGE(TAG, "Self test failed - gyro (%x)", err);
+	// }
+	// // Run accel self-test
+	// if ((err = self_test_accel()))
+	// {
+	// 	ESP_LOGE(TAG, "Self test failed - accel (%x)", err);
+	// }
+
+	// Run accel and gyro self-test
+	// TODO: REPAIR
+	// if ((err = self_test_accel_gyro()))
+	// {
+	// 	ESP_LOGE(TAG, "Self test failed - accel and gyro (%x)", err);
+	// }
 
 	// Run mag self-test
+<<<<<<< HEAD
 	// if ((err = self_test_mag()))
 	// {
 	// 	ESP_LOGE(TAG, "Self test failed - mag (%x)", err);
 	// }
+=======
+	if ((err = self_test_mag()))
+	{
+		ESP_LOGE(TAG, "Self test failed - mag (%x)", err);
+		return err;
+	}
+>>>>>>> imu_subsystems
 
 	return BIODYN_IMU_OK;
 }

main/imu/imu_icm20948_driver.c

@@ -191,5 +191,6 @@ biodyn_imu_err_t biodyn_imu_icm20948_config_gyro_sample_averaging(uint8_t gyro_s
 biodyn_imu_err_t biodyn_imu_icm20948_config_gyro_sample_averaging(uint8_t gyro_smplrt_div);
 
 // TODO rest of functions for configuring IMU
+// TODO remove bias from gyro and mag
 
 #endif // ICM20948_DRIVER_H