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/next' into brian/rfm22_FHSS

Browse Source
This commit is contained in:
Brian Webb 2013-03-09 01:43:46 +00:00
commit e70f0f2c21
18 changed files with 342 additions and 142 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

@ -147,6 +147,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,
@ -181,14 +182,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
};
@ -740,6 +743,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) )

4
flight/board_hw_defs/revomini/board_hw_defs.c
View File

@ -1331,13 +1331,13 @@ static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
.pin_source = GPIO_PinSource1,
.pin_source = GPIO_PinSource0,
},
.remap = GPIO_AF_TIM5,
},

21
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp
View File

@ -45,8 +45,6 @@ UAVGadgetDecorator::UAVGadgetDecorator(IUAVGadget *gadget, QList<IUAVGadgetConfi
foreach (IUAVGadgetConfiguration *config, *m_configurations)
m_toolbar->addItem(config->name());
connect(m_toolbar, SIGNAL(activated(int)), this, SLOT(loadConfiguration(int)));
if (m_configurations->count() > 0)
loadConfiguration(0);
updateToolbar();
}
@ -57,7 +55,7 @@ UAVGadgetDecorator::~UAVGadgetDecorator()
}
void UAVGadgetDecorator::loadConfiguration(int index) {
IUAVGadgetConfiguration* config = m_configurations->at(index);
IUAVGadgetConfiguration *config = m_configurations->at(index);
loadConfiguration(config);
}
@ -70,13 +68,13 @@ void UAVGadgetDecorator::loadConfiguration(IUAVGadgetConfiguration *config)
}
void UAVGadgetDecorator::configurationChanged(IUAVGadgetConfiguration* config)
void UAVGadgetDecorator::configurationChanged(IUAVGadgetConfiguration *config)
{
if (config == m_activeConfiguration)
loadConfiguration(config);
}
void UAVGadgetDecorator::configurationAdded(IUAVGadgetConfiguration* config)
void UAVGadgetDecorator::configurationAdded(IUAVGadgetConfiguration *config)
{
if (config->classId() != classId())
return;
@ -85,7 +83,7 @@ void UAVGadgetDecorator::configurationAdded(IUAVGadgetConfiguration* config)
updateToolbar();
}
void UAVGadgetDecorator::configurationToBeDeleted(IUAVGadgetConfiguration* config)
void UAVGadgetDecorator::configurationToBeDeleted(IUAVGadgetConfiguration *config)
{
if (config->classId() != classId())
return;
@ -97,7 +95,7 @@ void UAVGadgetDecorator::configurationToBeDeleted(IUAVGadgetConfiguration* confi
updateToolbar();
}
void UAVGadgetDecorator::configurationNameChanged(IUAVGadgetConfiguration* config, QString oldName, QString newName)
void UAVGadgetDecorator::configurationNameChanged(IUAVGadgetConfiguration *config, QString oldName, QString newName)
{
if (config->classId() != classId())
return;
@ -112,20 +110,21 @@ void UAVGadgetDecorator::updateToolbar()
m_toolbar->setEnabled(m_toolbar->count() > 1);
}
void UAVGadgetDecorator::saveState(QSettings* qSettings)
void UAVGadgetDecorator::saveState(QSettings *qSettings)
{
if (m_activeConfiguration) {
qSettings->setValue("activeConfiguration",m_activeConfiguration->name());
qSettings->setValue("activeConfiguration", m_activeConfiguration->name());
}
}
void UAVGadgetDecorator::restoreState(QSettings* qSetting)
void UAVGadgetDecorator::restoreState(QSettings *qSettings)
{
QString configName = qSetting->value("activeConfiguration").toString();
QString configName = qSettings->value("activeConfiguration").toString();
foreach (IUAVGadgetConfiguration *config, *m_configurations) {
if (config->name() == configName) {
m_activeConfiguration = config;
loadConfiguration(config);
break;
}
}
}

8
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp
View File

@ -262,13 +262,17 @@ void UAVGadgetInstanceManager::createOptionsPages()
}
IUAVGadget *UAVGadgetInstanceManager::createGadget(QString classId, QWidget *parent)
IUAVGadget *UAVGadgetInstanceManager::createGadget(QString classId, QWidget *parent, bool loadDefaultConfiguration)
{
IUAVGadgetFactory *f = factory(classId);
if (f) {
QList<IUAVGadgetConfiguration*> *configs = configurations(classId);
IUAVGadget *g = f->createGadget(parent);
IUAVGadget *gadget = new UAVGadgetDecorator(g, configs);
UAVGadgetDecorator *gadget = new UAVGadgetDecorator(g, configs);
if ((loadDefaultConfiguration && configs && configs->count()) > 0) {
gadget->loadConfiguration(configs->at(0));
}
m_gadgetInstances.append(gadget);
connect(this, SIGNAL(configurationAdded(IUAVGadgetConfiguration*)), gadget, SLOT(configurationAdded(IUAVGadgetConfiguration*)));
connect(this, SIGNAL(configurationChanged(IUAVGadgetConfiguration*)), gadget, SLOT(configurationChanged(IUAVGadgetConfiguration*)));

2
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.h
View File

@ -61,7 +61,7 @@ public:
~UAVGadgetInstanceManager();
void readSettings(QSettings *qs);
void saveSettings(QSettings *qs);
IUAVGadget *createGadget(QString classId, QWidget *parent);
IUAVGadget *createGadget(QString classId, QWidget *parent, bool loadDefaultConfiguration = true);
void removeGadget(IUAVGadget *gadget);
void removeAllGadgets();
bool canDeleteConfiguration(IUAVGadgetConfiguration *config);

15
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp
View File

@ -347,11 +347,7 @@ void SplitterOrView::saveState(QSettings* qSettings) const {
static_cast<SplitterOrView*>(m_splitter->widget(1))->saveState(qSettings);
qSettings->endGroup();
} else if (gadget()) {
qSettings->setValue("type", "uavGadget");
qSettings->setValue("classId", gadget()->classId());
qSettings->beginGroup("gadget");
gadget()->saveState(qSettings);
qSettings->endGroup();
m_view->saveState(qSettings);
}
}
@ -374,13 +370,6 @@ void SplitterOrView::restoreState(QSettings* qSettings)
static_cast<SplitterOrView*>(m_splitter->widget(1))->restoreState(qSettings);
qSettings->endGroup();
} else if (mode == "uavGadget") {
QString classId = qSettings->value("classId").toString();
int index = m_view->indexOfClassId(classId);
m_view->listSelectionActivated(index);
if(qSettings->childGroups().contains("gadget")) {
qSettings->beginGroup("gadget");
gadget()->restoreState(qSettings);
qSettings->endGroup();
}
m_view->restoreState(qSettings);
}
}

43
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.cpp
View File

@ -230,14 +230,17 @@ void UAVGadgetView::updateToolBar()
void UAVGadgetView::listSelectionActivated(int index)
{
if (index < 0) // this could happen when called from SplitterOrView::restoreState()
if (index < 0 || index >= m_uavGadgetList->count())
index = m_defaultIndex;
QString classId = m_uavGadgetList->itemData(index).toString();
if (m_uavGadget && (m_uavGadget->classId() == classId))
return;
UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager();
IUAVGadget *gadgetToRemove = m_uavGadget;
IUAVGadget *gadget = im->createGadget(classId, this);
IUAVGadget *gadgetToRemove = m_uavGadget;
setGadget(gadget);
m_uavGadgetManager->setCurrentGadget(gadget);
im->removeGadget(gadgetToRemove);
@ -252,3 +255,39 @@ void UAVGadgetView::currentGadgetChanged(IUAVGadget *gadget)
{
m_activeLabel->setVisible(m_uavGadget == gadget);
}
void UAVGadgetView::saveState(QSettings* qSettings)
{
qSettings->setValue("type", "uavGadget");
qSettings->setValue("classId", gadget()->classId());
qSettings->beginGroup("gadget");
gadget()->saveState(qSettings);
qSettings->endGroup();
}
void UAVGadgetView::restoreState(QSettings* qSettings)
{
QString classId = qSettings->value("classId").toString();
int index = indexOfClassId(classId);
if (index < 0) {
index = m_defaultIndex;
}
classId = m_uavGadgetList->itemData(index).toString();
IUAVGadget *newGadget;
UAVGadgetInstanceManager *im = ICore::instance()->uavGadgetInstanceManager();
if(qSettings->childGroups().contains("gadget")) {
newGadget = im->createGadget(classId, this, false);
qSettings->beginGroup("gadget");
newGadget->restoreState(qSettings);
qSettings->endGroup();
}
else {
newGadget = im->createGadget(classId, this);
}
IUAVGadget *gadgetToRemove = m_uavGadget;
setGadget(newGadget);
m_uavGadgetManager->setCurrentGadget(newGadget);
im->removeGadget(gadgetToRemove);
}

10
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/uavgadgetview.h
View File

@ -67,21 +67,25 @@ public:
UAVGadgetView(UAVGadgetManager *uavGadgetManager, IUAVGadget *uavGadget = 0, QWidget *parent = 0);
virtual ~UAVGadgetView();
void removeGadget();
IUAVGadget *gadget() const;
void setGadget(IUAVGadget *uavGadget);
bool hasGadget(IUAVGadget *uavGadget) const;
int indexOfClassId(QString classId);
void removeGadget();
void showToolbar(bool show);
void saveState(QSettings *qSettings);
void restoreState(QSettings *qSettings);
public slots:
void closeView();
void listSelectionActivated(int index);
private slots:
void listSelectionActivated(int index);
void currentGadgetChanged(IUAVGadget *gadget);
private:
int indexOfClassId(QString classId);
void updateToolBar();
QPointer<UAVGadgetManager> m_uavGadgetManager;

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>