add self-test #2 and tune accelerometer #5

modified: flight/modules/Sensors/sensors.c modified: flight/pios/common/pios_bmc050.c modified: flight/pios/inc/pios_bmc050.h
dibalavs · Nov 11, 2013 · e7402e7 · e7402e7
1 parent 4d765ad
commit e7402e7
Show file tree

Hide file tree

Showing 3 changed files with 79 additions and 5 deletions.
diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c
@@ -167,7 +167,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
     portTickType lastSysTime;
     uint32_t accel_samples = 0;
     uint32_t gyro_samples  = 0;
-    int32_t accel_accum[3] = { 0, 0, 0 };
+    float accel_accum[3] = { 0, 0, 0 };
     int32_t gyro_accum[3]  = { 0, 0, 0 };
     float gyro_scaling     = 0;
     float accel_scaling    = 0;

diff --git a/flight/pios/common/pios_bmc050.c b/flight/pios/common/pios_bmc050.c
@@ -32,6 +32,7 @@
 
 #ifdef PIOS_INCLUDE_BMC050
 
+#define GRAVITY_CONST 9.81f
 /* Global Variables */
 
 /* Local Types */
@@ -267,11 +268,21 @@ static float PIOS_BMC050_RangeToValue()
 	}
 }
 
-const float mg_per_lsb = 1.953125f;
+const float g_per_lsb = GRAVITY_CONST / 512.0f;
 const float temp_per_lsb = 0.5f;
 void NormalizeAccelData(struct pios_bmc050_raw_data *raw, struct pios_bmc050_accel_data *data)
 {
-	const float coeff_g = PIOS_BMC050_RangeToValue() * mg_per_lsb * 0.001f;
+	// check sign. we may have negative acceleration.
+	if (raw->accel_x & 0x0200)
+		raw->accel_x = raw->accel_x - 0x03ff;
+
+	if (raw->accel_y & 0x0200)
+		raw->accel_y = raw->accel_y - 0x03ff;
+
+	if (raw->accel_z & 0x0200)
+		raw->accel_z = raw->accel_z - 0x03ff;
+
+	const float coeff_g = PIOS_BMC050_RangeToValue() * g_per_lsb;
 	data->accel_x = coeff_g * raw->accel_x;
 	data->accel_y = coeff_g * raw->accel_y;
 	data->accel_z = coeff_g * raw->accel_z;
@@ -487,15 +498,70 @@ uint32_t PIOS_BMC050_GetUpdateAccelTimeoutuS()
 	return defaultTimeout * 1000;
 }
 
+void PIOS_BMC050_AccelObtainTestData(int32_t sign, struct pios_bmc050_accel_data* out)
+{
+	struct pios_bmc050_raw_data raw;
+
+	int32_t rx = 0;
+	PIOS_BMC050_SetReg(BMC_ACCEL_SELF_TEST_ADDR, sign | BMC_ACCEL_SELF_TEST_X_AXIS);
+	PIOS_DELAY_WaituS(PIOS_BMC050_GetUpdateAccelTimeoutuS());
+	while(((rx = PIOS_BMC050_GetReg(BMC_ACCEL_X_LSB_ADDR)) & BMC_ACCEL_DATA_READY_BIT) == 0);
+	raw.accel_x = (PIOS_BMC050_GetReg(BMC_ACCEL_X_LSB_ADDR + 1) << 2) | (rx >> 6);
+
+	PIOS_BMC050_SetReg(BMC_ACCEL_SELF_TEST_ADDR, sign | BMC_ACCEL_SELF_TEST_Y_AXIS);
+	PIOS_DELAY_WaituS(PIOS_BMC050_GetUpdateAccelTimeoutuS());
+	while(((rx = PIOS_BMC050_GetReg(BMC_ACCEL_Y_LSB_ADDR)) & BMC_ACCEL_DATA_READY_BIT) == 0);
+	raw.accel_y = (PIOS_BMC050_GetReg(BMC_ACCEL_Y_LSB_ADDR + 1) << 2) | (rx >> 6);
+
+	PIOS_BMC050_SetReg(BMC_ACCEL_SELF_TEST_ADDR, sign | BMC_ACCEL_SELF_TEST_Z_AXIS);
+	PIOS_DELAY_WaituS(PIOS_BMC050_GetUpdateAccelTimeoutuS());
+	while(((rx = PIOS_BMC050_GetReg(BMC_ACCEL_Z_LSB_ADDR)) & BMC_ACCEL_DATA_READY_BIT) == 0);
+	raw.accel_z = (PIOS_BMC050_GetReg(BMC_ACCEL_Z_LSB_ADDR + 1) << 2) | (rx >> 6);
+
+	NormalizeAccelData(&raw, out);
+}
+
 int32_t PIOS_BMC050_AccelTest()
 {
 	PIOS_BMC050_SetMag();
 	PIOS_BMC050_ReleaseBus();
 
+	// Check chipID
 	PIOS_BMC050_SetAccel();
 	uint32_t rx = PIOS_BMC050_GetReg(BMC_ACCEL_CHIPID_ADDR);
 	PIOS_Assert(rx == 0x03);
 
+	// accel sensor self-test
+
+	struct pios_bmc050_cfg testCfg;
+	testCfg.accel_range = BMC_ACCEL_RANGE_2G;
+	testCfg.accel_bandwidth = devAccel.cfg->accel_bandwidth;
+	const struct pios_bmc050_cfg* prev_cfg = devAccel.cfg;
+	devAccel.cfg = &testCfg;
+	PIOS_BMC050_SetReg(BMC_ACCEL_G_RANGE_ADDR, BMC_ACCEL_RANGE_2G);
+
+	//Part 1 positive sign test
+	struct pios_bmc050_accel_data positive;
+	PIOS_BMC050_AccelObtainTestData(BMC_ACCEL_SELF_TEST_POS_SIGN, &positive);
+
+	struct pios_bmc050_accel_data negative;
+	PIOS_BMC050_AccelObtainTestData(BMC_ACCEL_SELF_TEST_NEG_SIGN, &negative);
+
+	// Check if they are working
+
+	const float dx = -(negative.accel_x - positive.accel_x);
+	const float dy = -(negative.accel_y - positive.accel_y);
+	const float dz = -(negative.accel_z - positive.accel_z);
+
+	PIOS_Assert(dx >= 0.8f * GRAVITY_CONST);
+	PIOS_Assert(dy >= 0.8f * GRAVITY_CONST);
+	PIOS_Assert(dz >= 0.4f * GRAVITY_CONST);
+
+	PIOS_BMC050_SetReg(BMC_ACCEL_SELF_TEST_ADDR, BMC_ACCEL_SELF_TEST_DISABLED);
+	devAccel.cfg = prev_cfg;
+	PIOS_DELAY_WaituS(PIOS_BMC050_GetUpdateAccelTimeoutuS());
+	PIOS_BMC050_ConfigAccel(devAccel.cfg);
+
 	return 0;
 }
 

diff --git a/flight/pios/inc/pios_bmc050.h b/flight/pios/inc/pios_bmc050.h
@@ -46,6 +46,7 @@
 #define BMC_ACCEL_POWERMODE_ADDR 0x11
 #define BMC_ACCEL_CONFIG_ADDR    0x13
 #define BMC_ACCEL_RESET_ADDR     0x14
+#define BMC_ACCEL_SELF_TEST_ADDR 0x32
 
 // Magnetometer addresses
 #define BMC_MAG_CHIPID_ADDR      0x40
@@ -58,14 +59,21 @@
 #define BMC_MAG_REPETITION_XY     0x51
 #define BMC_MAG_REPETITION_Z      0x52
 
+enum { BMC_ACCEL_SELF_TEST_POS_SIGN = (0 << 2),
+	   BMC_ACCEL_SELF_TEST_NEG_SIGN = (1 << 2),
+	   BMC_ACCEL_SELF_TEST_DISABLED = 0,
+	   BMC_ACCEL_SELF_TEST_X_AXIS   = 1,
+	   BMC_ACCEL_SELF_TEST_Y_AXIS   = 2,
+	   BMC_ACCEL_SELF_TEST_Z_AXIS   = 3 };
+
+enum { BMC_MAG_DATA_READY_BIT = 0x01 };
+
 enum { BMC_ACCEL_RESET_VAL = 0xB6 };
 
 enum { BMC_MAG_RESET_VAL = 0x83 };
 
 enum { BMC_ACCEL_DATA_READY_BIT = 0x01 };
 
-enum { BMC_MAG_DATA_READY_BIT = 0x01 };
-
 /* Accel range  */
 enum bmc050_accel_range { BMC_ACCEL_RANGE_2G   = 0x03,
                           BMC_ACCEL_RANGE_4G   = 0x05,