rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

PIOS_MPU6000: Make the driver perform the rotation to bring it into the OP

Browse Source 
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
This commit is contained in:
James Cotton 2012-09-11 01:45:15 -05:00
parent 4cbaa6df88
commit 88b483f37d
7 changed files with 89 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/CopterControl/System/pios_board.c
View File

@ -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 */

12
flight/Modules/Attitude/attitude.c
View File

@ -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;

32
flight/Modules/Sensors/sensors.c
View File

@ -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};

61
flight/PiOS/Common/pios_mpu6000.c
View File

@ -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);

8
flight/PiOS/inc/pios_mpu6000.h
View File

@ -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 */

3
flight/RevoMini/System/pios_board.c
View File

@ -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 */

3
flight/Revolution/System/pios_board.c
View File

@ -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 */