From ce2d0fcbb1e21924311db7aab044b592909de762 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 17 Jan 2012 19:36:32 -0600 Subject: [PATCH] L3GD20 data now running through IRQ -> FIFO --- flight/Modules/Sensors/sensors.c | 12 +++--------- flight/PiOS/STM32F4xx/pios_l3gd20.c | 9 ++++++--- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index 4361d2848..72c4fce38 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -247,7 +247,7 @@ static void SensorsTask(void *parameters) #elif defined(PIOS_INCLUDE_L3GD20) struct pios_l3gd20_data gyro; count = 0; - /*while((read_good = PIOS_L3GD20_ReadFifo(&gyro)) != 0); + while((read_good = PIOS_L3GD20_ReadFifo(&gyro)) != 0); while(read_good == 0) { count++; @@ -256,14 +256,8 @@ static void SensorsTask(void *parameters) gyro_accum[2] += gyro.gyro_z; read_good = PIOS_L3GD20_ReadFifo(&gyro); - } */ - - PIOS_L3GD20_ReadGyros(&gyro); - gyro_accum[0] = gyro.gyro_x; - gyro_accum[1] = gyro.gyro_y; - gyro_accum[2] = gyro.gyro_z; - count = 1; - + } + gyro_samples = count; gyro_scaling = PIOS_L3GD20_GetScale(); diff --git a/flight/PiOS/STM32F4xx/pios_l3gd20.c b/flight/PiOS/STM32F4xx/pios_l3gd20.c index 932f66326..71e9f8eaa 100644 --- a/flight/PiOS/STM32F4xx/pios_l3gd20.c +++ b/flight/PiOS/STM32F4xx/pios_l3gd20.c @@ -77,6 +77,10 @@ void PIOS_L3GD20_Init(const struct pios_l3gd20_cfg * new_cfg) /* Enable and set EOC EXTI Interrupt to the lowest priority */ NVIC_Init(&cfg->eoc_irq.init); + + // An initial read is needed to get it running + struct pios_l3gd20_data data; + PIOS_L3GD20_ReadGyros(&data); } @@ -315,7 +319,6 @@ uint32_t l3gd20_fifo_full = 0; void PIOS_L3GD20_IRQHandler(void) { -/* struct pios_l3gd20_data data; PIOS_L3GD20_ReadGyros(&data); @@ -323,11 +326,11 @@ void PIOS_L3GD20_IRQHandler(void) if(fifoBuf_getFree(&pios_l3gd20_fifo) < sizeof(data)) { l3gd20_fifo_full++; - return; + return; } fifoBuf_putData(&pios_l3gd20_fifo, (uint8_t *) &data, sizeof(data)); -*/ + l3gd20_irq++; }