From 4791f690ad6317c88add20a9239dfd3c3344c691 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 3 Mar 2013 20:05:27 +0100 Subject: [PATCH] OP-848 Move the handling of the UAVO configuration outside of the PiOS layer a configuration helper header has been added to eliminate a lot of boilerplate code in *pios_board.c --- flight/CopterControl/System/pios_board.c | 8 +- flight/PiOS/Common/pios_mpu6000.c | 154 ++++++----------------- flight/PiOS/inc/pios_mpu6000.h | 4 +- flight/PiOS/inc/pios_mpu6000_config.h | 72 +++++++++++ flight/RevoMini/System/pios_board.c | 8 +- flight/Revolution/System/pios_board.c | 8 +- 6 files changed, 126 insertions(+), 128 deletions(-) create mode 100644 flight/PiOS/inc/pios_mpu6000_config.h diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 210d109b5..e3eb5826b 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -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, @@ -119,9 +120,9 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .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_FROM_SETTINGS, - .gyro_range = PIOS_MPU6000_SCALE_FROM_SETTINGS, - .filter = PIOS_MPU6000_LOWPASS_FROM_SETTINGS, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, .orientation = PIOS_MPU6000_TOP_180DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ @@ -819,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 */ diff --git a/flight/PiOS/Common/pios_mpu6000.c b/flight/PiOS/Common/pios_mpu6000.c index 5ba47685a..9ccbb644e 100644 --- a/flight/PiOS/Common/pios_mpu6000.c +++ b/flight/PiOS/Common/pios_mpu6000.c @@ -31,7 +31,6 @@ /* Project Includes */ #include "pios.h" -#include "mpu6000settings.h" #if defined(PIOS_INCLUDE_MPU6000) #include "fifo_buffer.h" @@ -50,6 +49,7 @@ struct mpu6000_dev { 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; }; @@ -64,10 +64,6 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg); static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_MPU6000_GetReg(uint8_t address); -static uint8_t getGyroRange(enum pios_mpu6000_range range); -static uint8_t getAccelRange(enum pios_mpu6000_accel_range accel); -static uint8_t getFilterSetting(enum pios_mpu6000_filter filter); - #define DEG_TO_RAD (M_PI / 180.0) #define GRAV 9.81f @@ -143,9 +139,6 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg) PIOS_MPU6000_Test(); - //initialize settings for acc/gyro Scale and filter - Mpu6000SettingsInitialize(); - // Reset chip while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0); PIOS_DELAY_WaitmS(300); @@ -173,29 +166,11 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg) // FIFO storage #if defined(PIOS_MPU6000_ACCEL) - // Set the accel range - dev->accel_range = getAccelRange(cfg->accel_range); - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, dev->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 - // Digital low-pass filter - uint8_t filterSetting; - filterSetting = getFilterSetting(cfg->filter); - - // Sample rate divider, chosen upon digital filtering settings - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG, - filterSetting == PIOS_MPU6000_LOWPASS_256_HZ ? - cfg->Smpl_rate_div_no_dlp : cfg->Smpl_rate_div_dlp) != 0); - - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0); - - // Gyro range - dev->gyro_range = getGyroRange(cfg->gyro_range); - while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, dev->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) ; @@ -212,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 @@ -350,7 +361,6 @@ float PIOS_MPU6000_GetScale() return 1.0f / 65.5f; case PIOS_MPU6000_SCALE_1000_DEG: return 1.0f / 32.8f; - case PIOS_MPU6000_SCALE_FROM_SETTINGS: case PIOS_MPU6000_SCALE_2000_DEG: return 1.0f / 16.4f; } @@ -365,7 +375,6 @@ float PIOS_MPU6000_GetAccelScale() case PIOS_MPU6000_ACCEL_4G: return GRAV / 8192.0f; case PIOS_MPU6000_ACCEL_8G: - case PIOS_MPU6000_ACCEL_FROM_SETTINGS: return GRAV / 4096.0f; case PIOS_MPU6000_ACCEL_16G: return GRAV / 2048.0f; @@ -542,93 +551,6 @@ bool PIOS_MPU6000_IRQHandler(void) return xHigherPriorityTaskWoken == pdTRUE; } -/** - * @brief Return the gyro range setting based on config and/or mpu6000settings. - * \return the chosen gyro range - */ -static uint8_t getGyroRange(enum pios_mpu6000_range gyro) -{ - // if the setting is overridden by the board config then use the board value - if (gyro != PIOS_MPU6000_SCALE_FROM_SETTINGS) - return(uint8_t) gyro; - - uint8_t gyroSettings; - Mpu6000SettingsGyroScaleGet(&gyroSettings); - - switch (gyroSettings) { - case MPU6000SETTINGS_GYROSCALE_SCALE_250: - return PIOS_MPU6000_SCALE_250_DEG; - case MPU6000SETTINGS_GYROSCALE_SCALE_500: - return PIOS_MPU6000_SCALE_500_DEG; - case MPU6000SETTINGS_GYROSCALE_SCALE_1000: - return PIOS_MPU6000_SCALE_1000_DEG; - case MPU6000SETTINGS_GYROSCALE_SCALE_2000: - return PIOS_MPU6000_SCALE_2000_DEG; - default: - return PIOS_MPU6000_SCALE_2000_DEG; - } -} - -/** - * @brief Return the accel range setting based on config and/or mpu6000settings. - * \return the chosen accel range - */ -static uint8_t getAccelRange(enum pios_mpu6000_accel_range accel) -{ - // if the setting is overridden by the board config then use the board value - if (accel != PIOS_MPU6000_ACCEL_FROM_SETTINGS) - return (uint8_t) accel; - - uint8_t accelSetting; - Mpu6000SettingsAccelScaleGet(&accelSetting); - - switch (accelSetting) { - case MPU6000SETTINGS_ACCELSCALE_SCALE_2G: - return PIOS_MPU6000_ACCEL_2G; - case MPU6000SETTINGS_ACCELSCALE_SCALE_4G: - return PIOS_MPU6000_ACCEL_4G; - case MPU6000SETTINGS_ACCELSCALE_SCALE_8G: - return PIOS_MPU6000_ACCEL_8G; - case MPU6000SETTINGS_ACCELSCALE_SCALE_16G: - return PIOS_MPU6000_ACCEL_16G; - default: - return PIOS_MPU6000_ACCEL_8G; - } -} - -/** - * @brief Return the filter settings based on config and/or mpu6000settings. - * \return the chosen filter settings - */ -static uint8_t getFilterSetting(enum pios_mpu6000_filter filter) -{ - // if the setting is overridden by the board config then use the board value - if (filter != PIOS_MPU6000_LOWPASS_FROM_SETTINGS) - return(uint8_t) filter; - - uint8_t filterSetting; - Mpu6000SettingsFilterSettingGet(&filterSetting); - - switch (filterSetting) { - case MPU6000SETTINGS_FILTERSETTING_LOWPASS_256_HZ: - return PIOS_MPU6000_LOWPASS_256_HZ; - case MPU6000SETTINGS_FILTERSETTING_LOWPASS_188_HZ: - return PIOS_MPU6000_LOWPASS_188_HZ; - case MPU6000SETTINGS_FILTERSETTING_LOWPASS_98_HZ: - return PIOS_MPU6000_LOWPASS_98_HZ; - case MPU6000SETTINGS_FILTERSETTING_LOWPASS_42_HZ: - return PIOS_MPU6000_LOWPASS_42_HZ; - case MPU6000SETTINGS_FILTERSETTING_LOWPASS_20_HZ: - return PIOS_MPU6000_LOWPASS_20_HZ; - case MPU6000SETTINGS_FILTERSETTING_LOWPASS_10_HZ: - return PIOS_MPU6000_LOWPASS_10_HZ; - case MPU6000SETTINGS_FILTERSETTING_LOWPASS_5_HZ: - return PIOS_MPU6000_LOWPASS_5_HZ; - default: - return PIOS_MPU6000_LOWPASS_256_HZ; - } -} - #endif /** diff --git a/flight/PiOS/inc/pios_mpu6000.h b/flight/PiOS/inc/pios_mpu6000.h index 6d92ada51..6f5e6772a 100644 --- a/flight/PiOS/inc/pios_mpu6000.h +++ b/flight/PiOS/inc/pios_mpu6000.h @@ -102,7 +102,6 @@ #define PIOS_MPU6000_PWRMGMT_STOP_CLK 0X07 enum pios_mpu6000_range { - PIOS_MPU6000_SCALE_FROM_SETTINGS = 0xFF, PIOS_MPU6000_SCALE_250_DEG = 0x00, PIOS_MPU6000_SCALE_500_DEG = 0x08, PIOS_MPU6000_SCALE_1000_DEG = 0x10, @@ -110,7 +109,6 @@ enum pios_mpu6000_range { }; enum pios_mpu6000_filter { - PIOS_MPU6000_LOWPASS_FROM_SETTINGS = 0xFF, PIOS_MPU6000_LOWPASS_256_HZ = 0x00, PIOS_MPU6000_LOWPASS_188_HZ = 0x01, PIOS_MPU6000_LOWPASS_98_HZ = 0x02, @@ -121,7 +119,6 @@ enum pios_mpu6000_filter { }; enum pios_mpu6000_accel_range { - PIOS_MPU6000_ACCEL_FROM_SETTINGS = 0xFF, PIOS_MPU6000_ACCEL_2G = 0x00, PIOS_MPU6000_ACCEL_4G = 0x08, PIOS_MPU6000_ACCEL_8G = 0x10, @@ -167,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(); diff --git a/flight/PiOS/inc/pios_mpu6000_config.h b/flight/PiOS/inc/pios_mpu6000_config.h new file mode 100644 index 000000000..f75ca0f7f --- /dev/null +++ b/flight/PiOS/inc/pios_mpu6000_config.h @@ -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 3-axis gyor function headers + * @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 */ + diff --git a/flight/RevoMini/System/pios_board.c b/flight/RevoMini/System/pios_board.c index 68aec9ff8..380f5fe59 100644 --- a/flight/RevoMini/System/pios_board.c +++ b/flight/RevoMini/System/pios_board.c @@ -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, @@ -183,9 +184,9 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .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_FROM_SETTINGS, - .gyro_range = PIOS_MPU6000_SCALE_FROM_SETTINGS, - .filter = PIOS_MPU6000_LOWPASS_FROM_SETTINGS, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, .orientation = PIOS_MPU6000_TOP_180DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ @@ -732,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 } diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index 45a9a08f8..6f61c6625 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -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, @@ -224,9 +225,9 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { .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_FROM_SETTINGS, - .gyro_range = PIOS_MPU6000_SCALE_FROM_SETTINGS, - .filter = PIOS_MPU6000_LOWPASS_FROM_SETTINGS, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, .orientation = PIOS_MPU6000_TOP_0DEG }; #endif /* PIOS_INCLUDE_MPU6000 */ @@ -874,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: