From 88b483f37d753f6145580a6a5f20842aae43ea19 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 11 Sep 2012 01:45:15 -0500 Subject: [PATCH] PIOS_MPU6000: Make the driver perform the rotation to bring it into the OP coordinate system Previously there were hacks spread throughout to deal with various ways of positioning the chips. Now the driver structure specifies the rotation of the chip relative to the board layout and the output X/Y/Z are already in OP convention. This flag seems to do the right thing for Revolution, CC3D, and RevoMini --- flight/CopterControl/System/pios_board.c | 3 +- flight/Modules/Attitude/attitude.c | 12 ++--- flight/Modules/Sensors/sensors.c | 32 ++++++------- flight/PiOS/Common/pios_mpu6000.c | 61 ++++++++++++++++++++---- flight/PiOS/inc/pios_mpu6000.h | 8 ++++ flight/RevoMini/System/pios_board.c | 3 +- flight/Revolution/System/pios_board.c | 3 +- 7 files changed, 89 insertions(+), 33 deletions(-) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index f59e7335a..1b6770820 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -114,7 +114,8 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, .accel_range = PIOS_MPU6000_ACCEL_8G, .gyro_range = PIOS_MPU6000_SCALE_500_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 6b0c16cec..d7597aa88 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -379,13 +379,13 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) if (GyrosReadOnly() || AccelsReadOnly()) return 0; - gyros[0] = -mpu6000_data.gyro_y * PIOS_MPU6000_GetScale(); - gyros[1] = -mpu6000_data.gyro_x * PIOS_MPU6000_GetScale(); - gyros[2] = -mpu6000_data.gyro_z * PIOS_MPU6000_GetScale(); + gyros[0] = mpu6000_data.gyro_x * PIOS_MPU6000_GetScale(); + gyros[1] = mpu6000_data.gyro_y * PIOS_MPU6000_GetScale(); + gyros[2] = mpu6000_data.gyro_z * PIOS_MPU6000_GetScale(); - accels[0] = -mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale(); - accels[1] = -mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale(); - accels[2] = -mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale(); + accels[0] = mpu6000_data.accel_x * PIOS_MPU6000_GetAccelScale(); + accels[1] = mpu6000_data.accel_y * PIOS_MPU6000_GetAccelScale(); + accels[2] = mpu6000_data.accel_z * PIOS_MPU6000_GetAccelScale(); gyrosData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; accelsData->temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index b9c3159de..9f16fb5f9 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -259,9 +259,9 @@ static void SensorsTask(void *parameters) while(read_good == 0) { count++; - accel_accum[0] += accel.x; - accel_accum[1] += accel.y; - accel_accum[2] += accel.z; + accel_accum[1] += accel.x; + accel_accum[0] += accel.y; + accel_accum[2] -= accel.z; read_good = PIOS_BMA180_ReadFifo(&accel); } @@ -284,9 +284,9 @@ static void SensorsTask(void *parameters) } gyro_samples = 1; - gyro_accum[0] += gyro.gyro_x; - gyro_accum[1] += gyro.gyro_y; - gyro_accum[2] += gyro.gyro_z; + gyro_accum[1] += gyro.gyro_x; + gyro_accum[0] += gyro.gyro_y; + gyro_accum[2] -= gyro.gyro_z; gyro_scaling = PIOS_L3GD20_GetScale(); @@ -303,12 +303,12 @@ static void SensorsTask(void *parameters) while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) { - gyro_accum[0] += -mpu6000_data.gyro_x; - gyro_accum[1] += -mpu6000_data.gyro_y; + gyro_accum[0] += mpu6000_data.gyro_x; + gyro_accum[1] += mpu6000_data.gyro_y; gyro_accum[2] += mpu6000_data.gyro_z; - accel_accum[0] += -mpu6000_data.accel_x; - accel_accum[1] += -mpu6000_data.accel_y; + accel_accum[0] += mpu6000_data.accel_x; + accel_accum[1] += mpu6000_data.accel_y; accel_accum[2] += mpu6000_data.accel_z; gyro_samples ++; @@ -334,9 +334,9 @@ static void SensorsTask(void *parameters) } // Scale the accels - float accels[3] = {(float) accel_accum[1] / accel_samples, - (float) accel_accum[0] / accel_samples, - -(float) accel_accum[2] / accel_samples}; + float accels[3] = {(float) accel_accum[0] / accel_samples, + (float) accel_accum[1] / accel_samples, + (float) accel_accum[2] / accel_samples}; float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0], accels[1] * accel_scaling * accel_scale[1] - accel_bias[1], accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]}; @@ -353,9 +353,9 @@ static void SensorsTask(void *parameters) AccelsSet(&accelsData); // Scale the gyros - float gyros[3] = {(float) gyro_accum[1] / gyro_samples, - (float) gyro_accum[0] / gyro_samples, - -(float) gyro_accum[2] / gyro_samples}; + float gyros[3] = {(float) gyro_accum[0] / gyro_samples, + (float) gyro_accum[1] / gyro_samples, + (float) gyro_accum[2] / gyro_samples}; float gyros_out[3] = {gyros[0] * gyro_scaling, gyros[1] * gyro_scaling, gyros[2] * gyro_scaling}; diff --git a/flight/PiOS/Common/pios_mpu6000.c b/flight/PiOS/Common/pios_mpu6000.c index 904e9402c..f1e39712f 100644 --- a/flight/PiOS/Common/pios_mpu6000.c +++ b/flight/PiOS/Common/pios_mpu6000.c @@ -276,6 +276,7 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data) */ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * data) { + // THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION uint8_t buf[7] = {PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0}; uint8_t rec[7]; @@ -451,19 +452,63 @@ void PIOS_MPU6000_IRQHandler(void) PIOS_MPU6000_ReleaseBus(); } + // Rotate the sensor to OP convention. The datasheet defines X as towards the right + // and Y as forward. OP convention transposes this. Also the Z is defined negatively + // to our convention #if defined(PIOS_MPU6000_ACCEL) - data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; - data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; - data.accel_z = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; + // Currently we only support rotations on top so switch X/Y accordingly + switch(dev->cfg->orientation) { + case PIOS_MPU6000_TOP_0DEG: + data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X + data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y + data.gyro_y = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X + data.gyro_x = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y + break; + case PIOS_MPU6000_TOP_90DEG: + data.accel_y = -(mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y + data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X + data.gyro_y = -(mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y + data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X + break; + case PIOS_MPU6000_TOP_180DEG: + data.accel_y = -(mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X + data.accel_x = -(mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y + data.gyro_y = -(mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X + data.gyro_x = -(mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y + break; + case PIOS_MPU6000_TOP_270DEG: + data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y + data.accel_x = -(mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X + data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y + data.gyro_x = -(mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X + break; + } + data.gyro_z = -(mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]); + data.accel_z = -(mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]; - data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; - data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; - data.gyro_z = mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]; #else - data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; - data.gyro_z = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]; + switch(dev->cfg->orientation) { + case PIOS_MPU6000_TOP_0DEG: + data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; + data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; + break; + case PIOS_MPU6000_TOP_90DEG: + data.gyro_y = -(mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y + data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X + break; + case PIOS_MPU6000_TOP_180DEG: + data.gyro_y = -(mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); + data.gyro_x = -(mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); + break; + case PIOS_MPU6000_TOP_270DEG: + data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y + data.gyro_x = -(mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X + break; + } + data.gyro_z = -(mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]); + data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; #endif xQueueSend(dev->queue, (void *) &data, 0); diff --git a/flight/PiOS/inc/pios_mpu6000.h b/flight/PiOS/inc/pios_mpu6000.h index 317b8155b..b8a49a1ad 100644 --- a/flight/PiOS/inc/pios_mpu6000.h +++ b/flight/PiOS/inc/pios_mpu6000.h @@ -123,6 +123,13 @@ enum pios_mpu6000_accel_range { PIOS_MPU6000_ACCEL_16G = 0x18 }; +enum pios_mpu6000_orientation { // clockwise rotation from board forward + PIOS_MPU6000_TOP_0DEG = 0x00, + PIOS_MPU6000_TOP_90DEG = 0x01, + PIOS_MPU6000_TOP_180DEG = 0x02, + PIOS_MPU6000_TOP_270DEG = 0x03 +}; + struct pios_mpu6000_data { int16_t gyro_x; int16_t gyro_y; @@ -147,6 +154,7 @@ struct pios_mpu6000_cfg { enum pios_mpu6000_accel_range accel_range; enum pios_mpu6000_range gyro_range; enum pios_mpu6000_filter filter; + enum pios_mpu6000_orientation orientation; }; /* Public Functions */ diff --git a/flight/RevoMini/System/pios_board.c b/flight/RevoMini/System/pios_board.c index 89ff48e58..76a81ce9f 100644 --- a/flight/RevoMini/System/pios_board.c +++ b/flight/RevoMini/System/pios_board.c @@ -183,7 +183,8 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, .accel_range = PIOS_MPU6000_ACCEL_8G, .gyro_range = PIOS_MPU6000_SCALE_500_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index b098f3cd0..a1f12f244 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -224,7 +224,8 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, .accel_range = PIOS_MPU6000_ACCEL_8G, .gyro_range = PIOS_MPU6000_SCALE_500_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_0DEG }; #endif /* PIOS_INCLUDE_MPU6000 */