diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 354bd7bbd..5ba2c3ad6 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -215,7 +215,7 @@ SRC += $(PIOSSTM32F10X)/pios_led.c SRC += $(PIOSSTM32F10X)/pios_delay.c SRC += $(PIOSSTM32F10X)/pios_usart.c SRC += $(PIOSSTM32F10X)/pios_irq.c -SRC += $(PIOSSTM32F10X)/pios_adc.c +#SRC += $(PIOSSTM32F10X)/pios_adc.c SRC += $(PIOSSTM32F10X)/pios_servo.c SRC += $(PIOSSTM32F10X)/pios_i2c.c SRC += $(PIOSSTM32F10X)/pios_spi.c @@ -248,8 +248,7 @@ SRC += $(PIOSCOMMON)/pios_crc.c SRC += $(PIOSCOMMON)/pios_flashfs_objlist.c SRC += $(PIOSCOMMON)/pios_flash_jedec.c SRC += $(PIOSCOMMON)/pios_adxl345.c -SRC += $(PIOSCOMMON)/pios_l3gd20.c -SRC += $(PIOSCOMMON)/pios_bma180.c +SRC += $(PIOSCOMMON)/pios_mpu6000.c SRC += $(PIOSCOMMON)/pios_com.c #SRC += $(PIOSCOMMON)/pios_i2c_esc.c #SRC += $(PIOSCOMMON)/pios_bmp085.c diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index dfe27bc3d..8552d441b 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -34,7 +34,7 @@ #define PIOS_CONFIG_H /* Enable/Disable PiOS Modules */ -#define PIOS_INCLUDE_ADC +//#define PIOS_INCLUDE_ADC #define PIOS_INCLUDE_DELAY //#if defined(USE_I2C) //#define PIOS_INCLUDE_I2C @@ -77,11 +77,8 @@ #define PIOS_INCLUDE_ADXL345 #define PIOS_INCLUDE_FLASH -#define PIOS_INCLUDE_BMA180 -#define PIOS_INCLUDE_L3GD20 -/* -#define PIOS_INCLUDE_BMP085 -*/ +#define PIOS_INCLUDE_MPU6000 +#define PIOS_MPU6000_ACCEL /* A really shitty setting saving implementation */ #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 484bedc5e..5ddca2ffc 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -68,52 +68,12 @@ uint32_t pios_com_gps_id; uint32_t pios_com_bridge_id; /** - * Configuration for the BMA180 chip + * Configuration for MPU6000 chip */ -#if defined(PIOS_INCLUDE_BMA180) -#include "pios_bma180.h" -static const struct pios_exti_cfg pios_exti_bma180_cfg __exti_config = { - .vector = PIOS_BMA180_IRQHandler, - .line = EXTI_Line13, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI15_10_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line13, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, -}; -static const struct pios_bma180_cfg pios_bma180_cfg = { - .exti_cfg = &pios_exti_bma180_cfg, - .bandwidth = BMA_BW_300HZ, - .range = BMA_RANGE_8G, -}; -#endif /* PIOS_INCLUDE_BMA180 */ - -/** - * Configuration for L3GD20 chip - */ -#if defined(PIOS_INCLUDE_L3GD20) -#include "pios_l3gd20.h" -static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = { - .vector = PIOS_L3GD20_IRQHandler, +#if defined(PIOS_INCLUDE_MPU6000) +#include "pios_mpu6000.h" +static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { + .vector = PIOS_MPU6000_IRQHandler, .line = EXTI_Line3, .pin = { .gpio = GPIOA, @@ -141,11 +101,19 @@ static const struct pios_exti_cfg pios_exti_l3gd20_cfg __exti_config = { }, }; -static const struct pios_l3gd20_cfg pios_l3gd20_cfg = { - .exti_cfg = &pios_exti_l3gd20_cfg, - .range = PIOS_L3GD20_SCALE_500_DEG, +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 = 7, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .gyro_range = PIOS_MPU6000_SCALE_500_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ }; -#endif /* PIOS_INCLUDE_L3GD20 */ +#endif /* PIOS_INCLUDE_MPU6000 */ static const struct flashfs_cfg flashfs_w25x_cfg = { .table_magic = 0x85FB3C35, @@ -160,12 +128,25 @@ static const struct pios_flash_jedec_cfg flash_w25x_cfg = { .chip_erase = 0x60 }; +static const struct flashfs_cfg flashfs_m25p_cfg = { + .table_magic = 0x85FB3D35, + .obj_magic = 0x3015A371, + .obj_table_start = 0x00000010, + .obj_table_end = 0x00010000, + .sector_size = 0x00010000, +}; + +static const struct pios_flash_jedec_cfg flash_m25p_cfg = { + .sector_erase = 0xD8, + .chip_erase = 0xC7 +}; #include /** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ +int32_t init_test; void PIOS_Board_Init(void) { /* Delay system */ @@ -206,9 +187,19 @@ void PIOS_Board_Init(void) { } #endif - PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 1, &flash_w25x_cfg); - PIOS_FLASHFS_Init(&flashfs_w25x_cfg); + switch(bdinfo->board_rev) { + case 0x01: // Revision 1 + PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 1, &flash_w25x_cfg); + PIOS_FLASHFS_Init(&flashfs_w25x_cfg); + break; + case 0x02: // Revision 2 + PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 0, &flash_m25p_cfg); + PIOS_FLASHFS_Init(&flashfs_m25p_cfg); + break; + default: + PIOS_DEBUG_Assert(0); + } /* Initialize UAVObject libraries */ EventDispatcherInitialize(); @@ -709,7 +700,9 @@ void PIOS_Board_Init(void) { switch(bdinfo->board_rev) { case 0x01: // Revision 1 with invensense gyros, start the ADC +#if defined(PIOS_INCLUDE_ADC) PIOS_ADC_Init(); +#endif #if defined(PIOS_INCLUDE_ADXL345) PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); #endif @@ -718,19 +711,16 @@ void PIOS_Board_Init(void) { // Revision 2 with L3GD20 gyros, start a SPI interface and connect to it GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); -#if defined(PIOS_INCLUDE_L3GD20) +#if defined(PIOS_INCLUDE_MPU6000) // Set up the SPI interface to the serial flash if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_Assert(0); } - PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg); - PIOS_Assert(PIOS_L3GD20_Test() == 0); -#endif /* PIOS_INCLUDE_L3GD20 */ + PIOS_MPU6000_Attach(pios_spi_gyro_id); + PIOS_MPU6000_Init(&pios_mpu6000_cfg); + init_test = PIOS_MPU6000_Test(); +#endif /* PIOS_INCLUDE_MPU6000 */ -#if defined(PIOS_INCLUDE_BMA180) - PIOS_BMA180_Init(pios_spi_flash_accel_id, 0, &pios_bma180_cfg); - PIOS_Assert(PIOS_BMA180_Test() == 0); -#endif /* PIOS_INCLUDE_BMA180 */ break; default: PIOS_Assert(0); diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index e8ef24a72..bdd03f146 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -63,6 +63,7 @@ #define STACK_SIZE_BYTES 540 #define TASK_PRIORITY (tskIDLE_PRIORITY+3) +#define SENSOR_PERIOD 2 #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 @@ -183,22 +184,21 @@ static void AttitudeTask(void *parameters) bool cc3d = bdinfo->board_rev == 0x02; if(cc3d) { -#if defined(PIOS_INCLUDE_BMA180) - accel_test = PIOS_BMA180_Test(); -#endif -#if defined(PIOS_INCLUDE_L3GD20) - gyro_test = PIOS_L3GD20_Test(); +#if defined(PIOS_INCLUDE_MPU6000) + gyro_test = PIOS_MPU6000_Test(); #endif } else { #if defined(PIOS_INCLUDE_ADXL345) accel_test = PIOS_ADXL345_Test(); #endif - + +#if defined(PIOS_INCLUDE_ADC) // Create queue for passing gyro data, allow 2 back samples in case gyro_queue = xQueueCreate(1, sizeof(float) * 4); PIOS_Assert(gyro_queue != NULL); PIOS_ADC_SetQueue(gyro_queue); PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); +#endif } // Force settings update to make sure rotation loaded @@ -245,7 +245,6 @@ static void AttitudeTask(void *parameters) else { // Only update attitude when sensor data is good updateAttitude(&accels, &gyros); - AccelsSet(&accels); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } } @@ -345,9 +344,8 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros) // and make it average zero (weakly) gyro_correct_int[2] += - gyros->z * yawBiasRate; - gyros_passed[0] = gyros->x; - gyros_passed[1] = gyros->y; - gyros_passed[2] = gyros->z; + GyrosSet(gyros); + AccelsSet(accels); return 0; } @@ -357,111 +355,94 @@ static int32_t updateSensors(AccelsData * accels, GyrosData * gyros) * @param[in] attitudeRaw Populate the UAVO instead of saving right here * @return 0 if successfull, -1 if not */ -struct pios_bma180_data accel; -struct pios_l3gd20_data gyro; +struct pios_mpu6000_data gyro; static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) { static portTickType lastSysTime; uint32_t accel_samples = 0; int32_t accel_accum[3] = {0, 0, 0}; float gyro_scaling = 1; - float gyros_accum[3] = {0, 0, 0}; + float gyro_accum[3] = {0, 0, 0}; float accel_scaling = 1; + uint32_t gyro_samples; -#if defined(PIOS_INCLUDE_L3GD20) - xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); - struct pios_l3gd20_data gyro; - gyro_scaling = PIOS_L3GD20_GetScale(); + vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); - if(xQueueReceive(gyro_queue, (void *) &gyro, 3) == errQUEUE_EMPTY) { - // Unfortunately if the L3GD20 ever misses getting read, then it will not - // trigger more interrupts. In this case we must force a read to kickstart - // it. - PIOS_L3GD20_ReadGyros(&gyro); - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return -1; - } - - gyrosData->x = gyro.gyro_x * gyro_scaling; - gyrosData->y = -gyro.gyro_y * gyro_scaling; - gyrosData->z = -gyro.gyro_z * gyro_scaling; - gyrosData->temperature = 0; - GyrosSet(gyrosData); - - gyros_accum[0] = gyrosData->x; - gyros_accum[1] = gyrosData->y; - gyros_accum[2] = gyrosData->z; +#if defined(PIOS_INCLUDE_MPU6000) - if(xQueueReceive(gyro_queue, (void * const) &gyro, 3) == errQUEUE_EMPTY) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + uint32_t count = 0; + int32_t read_good = 0; + int32_t error = false; + + while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0 && !error) + error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; + if (error) return -1; - } - - gyrosData->x = gyro.gyro_x * gyro_scaling; - gyrosData->y = -gyro.gyro_y * gyro_scaling; - gyrosData->z = -gyro.gyro_z * gyro_scaling; - gyrosData->temperature = 0; - - gyros_accum[0] += gyrosData->x; - gyros_accum[1] += gyrosData->y; - gyros_accum[2] += gyrosData->z; - - if(xQueueReceive(gyro_queue, (void * const) &gyro, 3) == errQUEUE_EMPTY) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return -1; - } - - gyrosData->x = gyro.gyro_x * gyro_scaling; - gyrosData->y = -gyro.gyro_y * gyro_scaling; - gyrosData->z = -gyro.gyro_z * gyro_scaling; - gyrosData->temperature = 0; - GyrosSet(gyrosData); - - gyros_accum[0] += gyrosData->x; - gyros_accum[1] += gyrosData->y; - gyros_accum[2] += gyrosData->z; - - gyros_passed[0] = gyros_accum[0] / 3; - gyros_passed[1] = gyros_accum[1] / 3; - gyros_passed[2] = gyros_accum[2] / 3; -#endif - -#if defined(PIOS_INCLUDE_BMA180) - struct pios_bma180_data accel; - accel_samples = 0; - bool error = false; - int32_t accel_read_good; - - while((accel_read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) - error = ((xTaskGetTickCount() - lastSysTime) > 5) ? true : error; - if (error) { - // Unfortunately if the BMA180 ever misses getting read, then it will not - // trigger more interrupts. In this case we must force a read to kickstart - // it. - PIOS_BMA180_ReadAccels(&accel); - lastSysTime = xTaskGetTickCount(); - return -1; - } - while(accel_read_good == 0) { - accel_samples++; + while(read_good == 0) { + count++; - accel_accum[0] += accel.x; - accel_accum[1] += accel.y; - accel_accum[2] += accel.z; + gyro_accum[0] += gyro.gyro_x; + gyro_accum[1] += gyro.gyro_y; + gyro_accum[2] += gyro.gyro_z; - accel_read_good = PIOS_BMA180_ReadFifo(&accel); + accel_accum[0] += gyro.accel_x; + accel_accum[1] += gyro.accel_y; + accel_accum[2] += gyro.accel_z; + + read_good = PIOS_MPU6000_ReadFifo(&gyro); } - accel_scaling = PIOS_BMA180_GetScale(); + gyro_samples = count; + gyro_scaling = PIOS_MPU6000_GetScale(); + + accel_samples = count; + accel_scaling = PIOS_MPU6000_GetAccelScale(); #endif - - float accels[3] = {(float) -accel_accum[1] / accel_samples, (float) -accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples}; - + // Get temp from last reading + gyrosData->temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f; + accelsData->temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f; + + // Scale the accels + float accels[3] = {-(float) accel_accum[1] / accel_samples, + -(float) accel_accum[0] / accel_samples, + -(float) accel_accum[2] / accel_samples}; + + // Scale the gyros + float gyros[3] = {-(float) gyro_accum[1] / gyro_samples, + -(float) gyro_accum[0] / gyro_samples, + -(float) gyro_accum[2] / gyro_samples}; + + if(rotate) { + // TODO: rotate sensors too so stabilization is well behaved + float vec_out[3]; + rot_mult(R, accels, vec_out); + accels[0] = vec_out[0]; + accels[1] = vec_out[1]; + accels[2] = vec_out[2]; + rot_mult(R, gyros, vec_out); + gyros[0] = vec_out[0]; + gyros[1] = vec_out[1]; + gyros[2] = vec_out[2]; + } + accelsData->x = (accels[0] - accelbias[0]) * accel_scaling; accelsData->y = (accels[1] - accelbias[1]) * accel_scaling; accelsData->z = (accels[2] - accelbias[2]) * accel_scaling; - - lastSysTime = xTaskGetTickCount(); - + AccelsSet(&accelsData); + + gyrosData->x = gyros[0] * gyro_scaling; + gyrosData->y = gyros[1] * gyro_scaling; + gyrosData->z = gyros[2] * gyro_scaling; + + if(bias_correct_gyro) { + // Applying integral component here so it can be seen on the gyros and correct bias + gyrosData->x += gyro_correct_int[0]; + gyrosData->y += gyro_correct_int[1]; + gyrosData->z += gyro_correct_int[2]; + } + + GyrosSet(gyrosData); + AccelsSet(accelsData); + return 0; } @@ -475,7 +456,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) lastSysTime = thisSysTime; // Bad practice to assume structure order, but saves memory - float * gyros = gyros_passed; + float * gyros = &gyrosData->x; float * accels = &accelsData->x; float grot[3]; diff --git a/flight/PiOS/Common/pios_mpu6000.c b/flight/PiOS/Common/pios_mpu6000.c index fe4aba089..dfb1f2377 100644 --- a/flight/PiOS/Common/pios_mpu6000.c +++ b/flight/PiOS/Common/pios_mpu6000.c @@ -45,7 +45,7 @@ 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); -#define PIOS_MPU6000_MAX_DOWNSAMPLE 100 +#define PIOS_MPU6000_MAX_DOWNSAMPLE 10 static int16_t pios_mpu6000_buffer[PIOS_MPU6000_MAX_DOWNSAMPLE * sizeof(struct pios_mpu6000_data)]; static t_fifo_buffer pios_mpu6000_fifo; @@ -66,9 +66,9 @@ void PIOS_MPU6000_Init(const struct pios_mpu6000_cfg * new_cfg) fifoBuf_init(&pios_mpu6000_fifo, (uint8_t *) pios_mpu6000_buffer, sizeof(pios_mpu6000_buffer)); /* Configure the MPU6000 Sensor */ - PIOS_SPI_SetPrescalar(pios_spi_gyro, SPI_BaudRatePrescaler_256); + PIOS_SPI_SetClockSpeed(pios_spi_gyro, PIOS_SPI_PRESCALER_256); PIOS_MPU6000_Config(cfg); - PIOS_SPI_SetPrescalar(pios_spi_gyro, SPI_BaudRatePrescaler_8); + PIOS_SPI_SetClockSpeed(pios_spi_gyro, PIOS_SPI_PRESCALER_16); /* Set up EXTI line */ PIOS_EXTI_Init(cfg->exti_cfg); @@ -295,10 +295,11 @@ float PIOS_MPU6000_GetAccelScale() * \return 0 if test succeeded * \return non-zero value if test succeeded */ +int32_t mpu6000_id; uint8_t PIOS_MPU6000_Test(void) { /* Verify that ID matches (MPU6000 ID is 0x69) */ - int32_t mpu6000_id = PIOS_MPU6000_ReadID(); + mpu6000_id = PIOS_MPU6000_ReadID(); if(mpu6000_id < 0) return -1; @@ -371,9 +372,9 @@ void PIOS_MPU6000_IRQHandler(void) } PIOS_MPU6000_ReleaseBus(); - + struct pios_mpu6000_data data; - + if(fifoBuf_getFree(&pios_mpu6000_fifo) < sizeof(data)) { mpu6000_fifo_full++; return; @@ -395,8 +396,6 @@ void PIOS_MPU6000_IRQHandler(void) PIOS_MPU6000_ReleaseBus(); - struct pios_mpu6000_data data; - if(fifoBuf_getFree(&pios_mpu6000_fifo) < sizeof(data)) { mpu6000_fifo_full++; return; diff --git a/flight/board_hw_defs/coptercontrol/board_hw_defs.c b/flight/board_hw_defs/coptercontrol/board_hw_defs.c index 2abc9ad8b..c17bb32f3 100644 --- a/flight/board_hw_defs/coptercontrol/board_hw_defs.c +++ b/flight/board_hw_defs/coptercontrol/board_hw_defs.c @@ -88,7 +88,7 @@ static const struct pios_spi_cfg pios_spi_gyro_cfg = { .SPI_CRCPolynomial = 7, .SPI_CPOL = SPI_CPOL_High, .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, /* 10 Mhz */ + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */ }, .use_crc = false, .dma = { @@ -390,6 +390,7 @@ void PIOS_SPI_flash_accel_irq_handler(void) /* * ADC system */ +#if defined(PIOS_INCLUDE_ADC) #include "pios_adc_priv.h" extern void PIOS_ADC_handler(void); void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); @@ -437,6 +438,7 @@ uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); void PIOS_ADC_handler() { PIOS_ADC_DMA_Handler(); } +#endif #include "pios_tim_priv.h"