rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/amorale/op-848_mpu6000_configurability' into next

Browse Source
This commit is contained in:
Alessio Morale 2013-03-08 00:08:15 +01:00
commit ffbb0a5a8e
11 changed files with 273 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/CopterControl/Makefile
View File

@ -233,6 +233,7 @@ SRC += $(OPUAVSYNTHDIR)/ratedesired.c
SRC += $(OPUAVSYNTHDIR)/baroaltitude.c
SRC += $(OPUAVSYNTHDIR)/txpidsettings.c
SRC += $(OPUAVSYNTHDIR)/airspeedactual.c
SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
endif

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

@ -79,6 +79,7 @@ uint32_t pios_usb_rctx_id;
*/
#if defined(PIOS_INCLUDE_MPU6000)
#include "pios_mpu6000.h"
#include "pios_mpu6000_config.h"
static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
.vector = PIOS_MPU6000_IRQHandler,
.line = EXTI_Line3,
@ -112,13 +113,15 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.exti_cfg = &pios_exti_mpu6000_cfg,
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
// Clock at 8 khz, downsampled by 16 for 500 Hz
.Smpl_rate_div = 15,
.Smpl_rate_div_no_dlp = 15,
// Clock at 1 khz, downsampled by 2 for 500 Hz
.Smpl_rate_div_dlp = 1,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_180DEG
};
@ -817,6 +820,7 @@ void PIOS_Board_Init(void) {
PIOS_Assert(0);
}
PIOS_MPU6000_Init(pios_spi_gyro_id,0,&pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
init_test = PIOS_MPU6000_Test();
#endif /* PIOS_INCLUDE_MPU6000 */

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

@ -31,7 +31,6 @@
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_MPU6000)
#include "fifo_buffer.h"
@ -48,6 +47,9 @@ struct mpu6000_dev {
uint32_t slave_num;
xQueueHandle queue;
const struct pios_mpu6000_cfg * cfg;
enum pios_mpu6000_range gyro_range;
enum pios_mpu6000_accel_range accel_range;
enum pios_mpu6000_filter filter;
enum pios_mpu6000_dev_magic magic;
};
@ -130,51 +132,45 @@ int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios
* @brief Initialize the MPU6000 3-axis gyro sensor
* \return none
* \param[in] PIOS_MPU6000_ConfigTypeDef struct to be used to configure sensor.
*
*/
*
*/
static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
{
PIOS_MPU6000_Test();
// Reset chip
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, 0x80) != 0);
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0);
PIOS_DELAY_WaitmS(300);
// Reset chip and fifo
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, 0x80 | 0x01 | 0x02 | 0x04) != 0);
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG,
PIOS_MPU6000_USERCTL_GYRO_RST |
PIOS_MPU6000_USERCTL_SIG_COND |
PIOS_MPU6000_USERCTL_FIFO_RST) != 0);
// Wait for reset to finish
while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) & 0x07);
while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) &
(PIOS_MPU6000_USERCTL_GYRO_RST |
PIOS_MPU6000_USERCTL_SIG_COND |
PIOS_MPU6000_USERCTL_FIFO_RST));
//Power management configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) ;
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0);
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) ;
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0);
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) ;
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0);
// FIFO storage
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel scale
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, cfg->accel_range) != 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) ;
// Digital low-pass filter and scale
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, cfg->filter) != 0) ;
// Digital low-pass filter and scale
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, cfg->gyro_range) != 0) ;
PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter);
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) ;
@ -191,6 +187,42 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
mpu6000_configured = true;
}
/**
* @brief Configures Gyro, accel and Filter ranges/setings
* @return 0 if successful, -1 if device has not been initialized
*/
int32_t PIOS_MPU6000_ConfigureRanges(
enum pios_mpu6000_range gyroRange,
enum pios_mpu6000_accel_range accelRange,
enum pios_mpu6000_filter filterSetting
)
{
if(dev == NULL)
return -1;
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
// update filter settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0);
// Sample rate divider, chosen upon digital filtering settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG,
filterSetting == PIOS_MPU6000_LOWPASS_256_HZ ?
dev->cfg->Smpl_rate_div_no_dlp : dev->cfg->Smpl_rate_div_dlp) != 0);
dev->filter = filterSetting;
// Gyro range
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, gyroRange) != 0);
dev->gyro_range = gyroRange;
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel range
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0);
dev->accel_range = accelRange;
#endif
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
return 0;
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
@ -322,7 +354,7 @@ xQueueHandle PIOS_MPU6000_GetQueue()
float PIOS_MPU6000_GetScale()
{
switch (dev->cfg->gyro_range) {
switch (dev->gyro_range) {
case PIOS_MPU6000_SCALE_250_DEG:
return 1.0f / 131.0f;
case PIOS_MPU6000_SCALE_500_DEG:
@ -337,7 +369,7 @@ float PIOS_MPU6000_GetScale()
float PIOS_MPU6000_GetAccelScale()
{
switch (dev->cfg->accel_range) {
switch (dev->accel_range) {
case PIOS_MPU6000_ACCEL_2G:
return GRAV / 16384.0f;
case PIOS_MPU6000_ACCEL_4G:
@ -411,20 +443,20 @@ bool PIOS_MPU6000_IRQHandler(void)
mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if(!mpu6000_configured)
if (!mpu6000_configured)
return false;
mpu6000_count = PIOS_MPU6000_FifoDepth();
if(mpu6000_count < sizeof(struct pios_mpu6000_data))
if (mpu6000_count < sizeof(struct pios_mpu6000_data))
return false;
if(PIOS_MPU6000_ClaimBus() != 0)
if (PIOS_MPU6000_ClaimBus() != 0)
return false;
uint8_t mpu6000_send_buf[1+sizeof(struct pios_mpu6000_data)] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t mpu6000_rec_buf[1+sizeof(struct pios_mpu6000_data)];
if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data) ] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t mpu6000_rec_buf[1 + sizeof(struct pios_mpu6000_data) ];
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBus();
mpu6000_fails++;
return false;
@ -437,85 +469,86 @@ bool PIOS_MPU6000_IRQHandler(void)
// In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= (sizeof(data) * 2)) {
mpu6000_fifo_backup++;
if(PIOS_MPU6000_ClaimBus() != 0)
return false;
if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
if (PIOS_MPU6000_ClaimBus() != 0)
return false;
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBus();
mpu6000_fails++;
return false;
}
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)
// 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;
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:
// -1 to bring it back to -32768 +32767 range
data.accel_y = -1 - (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 = -1 - (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 = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X
data.accel_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y
data.gyro_y = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X
data.gyro_x = -1 - (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 = -1 - (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 = -1 - (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.gyro_z = -1 - (mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]);
data.accel_z = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]);
data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8];
#else
data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
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;
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 = -1 - (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 = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]);
data.gyro_x = -1 - (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 = -1 - (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.gyro_z = -1 - (mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]);
data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
#endif
portBASE_TYPE xHigherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *) &data, &xHigherPriorityTaskWoken);
mpu6000_irq++;
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
return xHigherPriorityTaskWoken == pdTRUE;
return xHigherPriorityTaskWoken == pdTRUE;
}
#endif

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

@ -8,7 +8,7 @@
*
* @file PIOS_MPU6000.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief MPU6000 3-axis gyor function headers
* @brief MPU6000 3-axis gyro function headers
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************
@ -87,8 +87,10 @@
/* User control functionality */
#define PIOS_MPU6000_USERCTL_FIFO_EN 0X40
#define PIOS_MPU6000_USERCTL_I2C_MST_EN 0x20
#define PIOS_MPU6000_USERCTL_DIS_I2C 0X10
#define PIOS_MPU6000_USERCTL_FIFO_RST 0X02
#define PIOS_MPU6000_USERCTL_FIFO_RST 0X04
#define PIOS_MPU6000_USERCTL_SIG_COND 0X02
#define PIOS_MPU6000_USERCTL_GYRO_RST 0X01
/* Power management and clock selection */
@ -146,8 +148,11 @@ struct pios_mpu6000_cfg {
const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */
uint8_t Fifo_store; /* FIFO storage of different readings (See datasheet page 31 for more details) */
uint8_t Smpl_rate_div; /* Sample rate divider to use (See datasheet page 32 for more details) */
uint8_t interrupt_cfg; /* Interrupt configuration (See datasheet page 35 for more details) */
/* Sample rate divider to use (See datasheet page 32 for more details).*/
uint8_t Smpl_rate_div_no_dlp; /* used when no dlp is applied (fs=8KHz)*/
uint8_t Smpl_rate_div_dlp; /* used when dlp is on (fs=1kHz)*/
uint8_t interrupt_cfg; /* Interrupt configuration (See datasheet page 35 for more details) */
uint8_t interrupt_en; /* Interrupt configuration (See datasheet page 35 for more details) */
uint8_t User_ctl; /* User control settings (See datasheet page 41 for more details) */
uint8_t Pwr_mgmt_clk; /* Power management and clock selection (See datasheet page 32 for more details) */
@ -159,6 +164,7 @@ struct pios_mpu6000_cfg {
/* Public Functions */
extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg * new_cfg);
extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange,enum pios_mpu6000_filter filterSetting);
extern xQueueHandle PIOS_MPU6000_GetQueue();
extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * buffer);
extern int32_t PIOS_MPU6000_ReadID();

72
flight/PiOS/inc/pios_mpu6000_config.h Normal file
View File

@ -0,0 +1,72 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_MPU6000 OpenPilot layer configuration utilities
* @brief provides mpu6000 configuration helpers function
* @{
*
* @file PIOS_MPU6000_CONFIG.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief MPU6000 UAVO-based configuration functions
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************
*/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_MPU6000_CONFIG_H
#define PIOS_MPU6000_CONFIG_H
#include "mpu6000settings.h"
#include "pios_mpu6000.h"
#define PIOS_MPU6000_CONFIG_MAP_GYROSCALE(x) (x == MPU6000SETTINGS_GYROSCALE_SCALE_250 ? PIOS_MPU6000_SCALE_250_DEG : \
x == MPU6000SETTINGS_GYROSCALE_SCALE_500 ? PIOS_MPU6000_SCALE_500_DEG : \
x == MPU6000SETTINGS_GYROSCALE_SCALE_1000 ? PIOS_MPU6000_SCALE_1000_DEG : \
PIOS_MPU6000_SCALE_2000_DEG)
#define PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(x) (x == MPU6000SETTINGS_ACCELSCALE_SCALE_2G ? PIOS_MPU6000_ACCEL_2G : \
x == MPU6000SETTINGS_ACCELSCALE_SCALE_4G ? PIOS_MPU6000_ACCEL_4G : \
x == MPU6000SETTINGS_ACCELSCALE_SCALE_16G ? PIOS_MPU6000_ACCEL_16G : \
PIOS_MPU6000_ACCEL_8G)
#define PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(x) (x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_188_HZ ? PIOS_MPU6000_LOWPASS_188_HZ : \
x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_98_HZ ? PIOS_MPU6000_LOWPASS_98_HZ : \
x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_42_HZ ? PIOS_MPU6000_LOWPASS_42_HZ : \
x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_20_HZ ? PIOS_MPU6000_LOWPASS_20_HZ : \
x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_10_HZ ? PIOS_MPU6000_LOWPASS_10_HZ : \
x == MPU6000SETTINGS_FILTERSETTING_LOWPASS_5_HZ ? PIOS_MPU6000_LOWPASS_5_HZ : \
PIOS_MPU6000_LOWPASS_256_HZ)
/**
* @brief Updates MPU6000 config based on Mpu6000Settings UAVO
* @returns 0 if succeed or -1 otherwise
*/
int32_t PIOS_MPU6000_CONFIG_Configure()
{
Mpu6000SettingsInitialize();
Mpu6000SettingsData mpu6000settings;
Mpu6000SettingsGet(&mpu6000settings);
return PIOS_MPU6000_ConfigureRanges(
PIOS_MPU6000_CONFIG_MAP_GYROSCALE (mpu6000settings.GyroScale),
PIOS_MPU6000_CONFIG_MAP_ACCELSCALE(mpu6000settings.AccelScale),
PIOS_MPU6000_CONFIG_MAP_FILTERSETTING(mpu6000settings.FilterSetting)
);
}
#endif /* PIOS_MPU6000_CONFIG_H */

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

@ -141,6 +141,7 @@ static const struct pios_ms5611_cfg pios_ms5611_cfg = {
*/
#if defined(PIOS_INCLUDE_MPU6000)
#include "pios_mpu6000.h"
#include "pios_mpu6000_config.h"
static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
.vector = PIOS_MPU6000_IRQHandler,
.line = EXTI_Line4,
@ -175,14 +176,16 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.exti_cfg = &pios_exti_mpu6000_cfg,
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
// Clock at 8 khz, downsampled by 8 for 1khz
.Smpl_rate_div = 11,
// Clock at 8 khz, downsampled by 12 for 666Hz
.Smpl_rate_div_no_dlp = 11,
// with dlp on output rate is 500Hz
.Smpl_rate_div_dlp = 1,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_180DEG
};
@ -730,6 +733,7 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
#endif
}

2
flight/RevoMini/UAVObjects.inc
View File

@ -94,7 +94,7 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )

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

@ -182,6 +182,7 @@ static const struct pios_bma180_cfg pios_bma180_cfg = {
*/
#if defined(PIOS_INCLUDE_MPU6000)
#include "pios_mpu6000.h"
#include "pios_mpu6000_config.h"
static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
.vector = PIOS_MPU6000_IRQHandler,
.line = EXTI_Line8,
@ -216,14 +217,16 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.exti_cfg = &pios_exti_mpu6000_cfg,
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
// Clock at 8 khz, downsampled by 8 for 1khz
.Smpl_rate_div = 11,
// Clock at 8 khz, downsampled by 12 for 666Hz
.Smpl_rate_div_no_dlp = 11,
// with dlp on output rate is 500Hz
.Smpl_rate_div_dlp = 1,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_500_DEG,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_0DEG
};
@ -872,6 +875,7 @@ void PIOS_Board_Init(void) {
case 0x02:
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
#endif
break;
default:

1
flight/Revolution/UAVObjects.inc
View File

@ -94,6 +94,7 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

6
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -99,7 +99,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/oplinkstatus.h \
$$UAVOBJECT_SYNTHETICS/osdsettings.h \
$$UAVOBJECT_SYNTHETICS/waypoint.h \
$$UAVOBJECT_SYNTHETICS/waypointactive.h
$$UAVOBJECT_SYNTHETICS/waypointactive.h \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.h
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \
@ -178,4 +179,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \
$$UAVOBJECT_SYNTHETICS/osdsettings.cpp \
$$UAVOBJECT_SYNTHETICS/waypoint.cpp \
$$UAVOBJECT_SYNTHETICS/waypointactive.cpp
$$UAVOBJECT_SYNTHETICS/waypointactive.cpp \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp

38
shared/uavobjectdefinition/mpu6000settings.xml Normal file
View File

@ -0,0 +1,38 @@
<xml>
<object name="Mpu6000Settings" singleinstance="true" settings="true" category="Sensors">
<description>Settings for the @ref MPU6000 sensor used on CC3D and Revolution. Reboot the board for this to takes effect</description>
<field name="GyroScale" units="deg/s" type="enum" elements="1" defaultvalue="Scale_2000">
<options>
<option>Scale_250</option>
<option>Scale_500</option>
<option>Scale_1000</option>
<option>Scale_2000</option>
</options>
</field>
<field name="AccelScale" units="g" type="enum" elements="1" defaultvalue="Scale_8g">
<options>
<option>Scale_2g</option>
<option>Scale_4g</option>
<option>Scale_8g</option>
<option>Scale_16g</option>
</options>
</field>
<field name="FilterSetting" units="Hz" type="enum" elements="1" defaultvalue="Lowpass_256_Hz">
<options>
<option>Lowpass_256_Hz</option>
<option>Lowpass_188_Hz</option>
<option>Lowpass_98_Hz</option>
<option>Lowpass_42_Hz</option>
<option>Lowpass_20_Hz</option>
<option>Lowpass_10_Hz</option>
<option>Lowpass_5_Hz</option>
</options>
</field>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>