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

Add option to switch to using the MPU6000 accel instead of BMA180. Disabled by

Browse Source 
default.
This commit is contained in:
James Cotton 2011-12-29 00:10:13 -06:00
parent c080080810
commit 25dfb5463e
5 changed files with 78 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -188,6 +188,7 @@ static void SensorsTask(void *parameters)
gyro_samples = 0;
// Make sure we get one sample
#if !defined(PIOS_MPU6000_ACCEL)
count = 0;
while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0);
while(read_good == 0) {
@ -199,20 +200,13 @@ static void SensorsTask(void *parameters)
read_good = PIOS_BMA180_ReadFifo(&accel);
}
accel_samples = count;
float accels[3] = {(float) accel_accum[1] / accel_samples, (float) accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples};
// Not the swaping of channel orders
scaling = PIOS_BMA180_GetScale();
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = accels[0] * scaling * accel_scale[0] - accel_bias[0];
accelsData.y = accels[1] * scaling * accel_scale[1] - accel_bias[1];
accelsData.z = accels[2] * scaling * accel_scale[2] - accel_bias[2];
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
AccelsSet(&accelsData);
accel_samples = count;
#endif
#if defined(PIOS_MPU6000_ACCEL)
accel_accum[0] = accel_accum[1] = accel_accum[2] = 0;
#endif
// Make sure we get one sample
count = 0;
while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0);
@ -223,12 +217,34 @@ static void SensorsTask(void *parameters)
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
#if defined(PIOS_MPU6000_ACCEL)
accel_accum[0] += gyro.accel_x;
accel_accum[1] += gyro.accel_y;
accel_accum[2] += gyro.accel_z;
accel_samples = count;
#endif
read_good = PIOS_MPU6000_ReadFifo(&gyro);
}
gyro_samples = count;
float accels[3] = {(float) accel_accum[1] / accel_samples, (float) accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples};
// Not the swaping of channel orders
#if defined(PIOS_MPU6000_ACCEL)
scaling = PIOS_MPU6000_GetAccelScale();
#else
scaling = PIOS_BMA180_GetScale();
#endif
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = accels[0] * scaling * accel_scale[0] - accel_bias[0];
accelsData.y = accels[1] * scaling * accel_scale[1] - accel_bias[1];
accelsData.z = accels[2] * scaling * accel_scale[2] - accel_bias[2];
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
AccelsSet(&accelsData);
float gyros[3] = {(float) gyro_accum[1] / gyro_samples, (float) gyro_accum[0] / gyro_samples, -(float) gyro_accum[2] / gyro_samples};
scaling = PIOS_MPU6000_GetScale();
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = gyros[0] * scaling;

17
flight/PiOS/STM32F4xx/pios_bma180.c
View File

@ -49,7 +49,8 @@ static t_fifo_buffer pios_bma180_fifo;
static const struct pios_bma180_cfg * dev_cfg;
static enum bma180_range range;
#define GRAV 9.81
#define GRAV 9.81f
/**
* @brief Initialize with good default settings
*/
@ -285,19 +286,19 @@ float PIOS_BMA180_GetScale()
{
switch (range) {
case BMA_RANGE_1G:
return GRAV / 8192.0;
return GRAV / 8192.0f;
case BMA_RANGE_1_5G:
return GRAV / 5460.0;
return GRAV / 5460.0f;
case BMA_RANGE_2G:
return GRAV / 4096.0;
return GRAV / 4096.0f;
case BMA_RANGE_3G:
return GRAV / 2730.0;
return GRAV / 2730.0f;
case BMA_RANGE_4G:
return GRAV / 2048.0;
return GRAV / 2048.0f;
case BMA_RANGE_8G:
return GRAV / 1024.0;
return GRAV / 1024.0f;
case BMA_RANGE_16G:
return GRAV / 512.0;
return GRAV / 512.0f;
}
return 0;
}

31
flight/PiOS/STM32F4xx/pios_mpu6000.c
View File

@ -51,6 +51,8 @@ volatile bool mpu6000_configured = false;
static struct pios_mpu6000_cfg const * cfg;
#define GRAV 9.81f
/**
* @brief Initialize the MPU6050 3-axis gyro sensor.
* @return none
@ -86,6 +88,10 @@ void PIOS_MPU6000_Init(const struct pios_mpu6000_cfg * new_cfg)
*/
static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
{
// Reset chip
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, 0x80) != 0);
PIOS_DELAY_WaitmS(100);
// Reset chip and fifo
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, 0x01 | 0x02 | 0x04) != 0);
// Wait for reset to finish
@ -101,7 +107,14 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) ;
// FIFO storage
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel to 8g mode
while(PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, 0x10) != 0);
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0);
#else
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0);
#endif
// Sample rate divider
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, cfg->Smpl_rate_div) != 0) ;
@ -277,6 +290,12 @@ float PIOS_MPU6000_GetScale()
}
return 0;
}
float PIOS_MPU6000_GetAccelScale()
{
return GRAV / 2048.0f;
}
/**
* @brief Run self-test operation.
* \return 0 if test succeeded
@ -391,11 +410,21 @@ void PIOS_MPU6000_IRQHandler(void)
}
#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];
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];
#endif
fifoBuf_putData(&pios_mpu6000_fifo, (uint8_t *) &data, sizeof(data));
mpu6000_irq++;

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

@ -70,7 +70,7 @@
#define PIOS_MPU6000_FIFO_GYRO_X_OUT 0x40
#define PIOS_MPU6000_FIFO_GYRO_Y_OUT 0x20
#define PIOS_MPU6000_FIFO_GYRO_Z_OUT 0x10
#define PIOS_MPU6000_ACCEL_OUT 0x04
#define PIOS_MPU6000_ACCEL_OUT 0x08
/* Interrupt Configuration */
#define PIOS_MPU6000_INT_ACTL 0x80
@ -120,9 +120,11 @@ struct pios_mpu6000_data {
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
/* int16_t accel_x;
#if defined(PIOS_MPU6000_ACCEL)
int16_t accel_x;
int16_t accel_y;
int16_t accel_z; */
int16_t accel_z;
#endif /* PIOS_MPU6000_ACCEL */
int16_t temperature;
};
@ -149,6 +151,7 @@ extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * buffer);
extern int32_t PIOS_MPU6000_ReadID();
extern uint8_t PIOS_MPU6000_Test();
extern float PIOS_MPU6000_GetScale();
extern float PIOS_MPU6000_GetAccelScale();
#endif /* PIOS_MPU6000_H */

1
flight/Revolution/System/inc/pios_config.h
View File

@ -58,6 +58,7 @@
#define PIOS_INCLUDE_BMA180
#define PIOS_INCLUDE_HMC5883
#define PIOS_INCLUDE_MPU6000
//#define PIOS_MPU6000_ACCEL
#define PIOS_INCLUDE_MS5611
//#define PIOS_INCLUDE_HCSR04