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

Try and get the MPU6000 buffer to run down. Staying high for some reason.

Browse Source
This commit is contained in:
James Cotton 2011-11-17 11:23:31 -06:00
parent 0efffef1fa
commit 79730d5afa
4 changed files with 57 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
flight/PiOS/STM32F4xx/pios_mpu6000.c
View File

@ -296,82 +296,83 @@ uint8_t PIOS_MPU6000_Test(void)
/** /**
* @brief IRQ Handler. Read all the data from onboard buffer * @brief IRQ Handler. Read all the data from onboard buffer
*/ */
#define MPU6000_MAX_BLOCK_READ 12
uint32_t mpu6000_irq = 0; uint32_t mpu6000_irq = 0;
uint32_t mpu6000_count; uint32_t mpu6000_count;
uint32_t mpu6000_fifo_full = 0; uint32_t mpu6000_fifo_full = 0;
uint8_t mpu6000_send_buf[1+MPU6000_MAX_BLOCK_READ * sizeof(struct pios_mpu6000_data)];
uint8_t mpu6000_rec_buf[1+MPU6000_MAX_BLOCK_READ * sizeof(struct pios_mpu6000_data)];
uint8_t mpu6000_last_read_count = 0;
uint32_t mpu6000_fails = 0;
uint32_t mpu6000_interval_us;
uint32_t mpu6000_time_us;
uint32_t mpu6000_transfer_size;
void PIOS_MPU6000_IRQHandler(void) void PIOS_MPU6000_IRQHandler(void)
{ {
if(!mpu6000_configured) if(!mpu6000_configured)
return; return;
struct pios_mpu6000_data data;
mpu6000_irq++;
if(PIOS_MPU6000_ClaimBus() != 0) if(PIOS_MPU6000_ClaimBus() != 0)
return; return;
static uint32_t timeval;
mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
// Get number of bytes in the fifo // Get number of bytes in the fifo
uint8_t buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0}; mpu6000_send_buf[0] = PIOS_MPU6000_FIFO_CNT_MSB | 0x80;
uint8_t rec[3]; mpu6000_send_buf[1] = mpu6000_send_buf[2] = 0;
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { if(PIOS_SPI_TransferBlock(pios_spi_gyro, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], 3, NULL) < 0) {
PIOS_MPU6000_ReleaseBus(); PIOS_MPU6000_ReleaseBus();
return; return;
} }
mpu6000_count = rec[1] << 8 | rec[2];
uint32_t count = mpu6000_count > 40 ? 40 : mpu6000_count;
while(count--) { mpu6000_count = (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]) / sizeof(struct pios_mpu6000_data);
mpu6000_last_read_count = mpu6000_count > MPU6000_MAX_BLOCK_READ ? MPU6000_MAX_BLOCK_READ : mpu6000_count;
if (mpu6000_count == 0) {
PIOS_MPU6000_ReleaseBus();
return;
}
mpu6000_last_read_count = 1;
mpu6000_send_buf[0] = PIOS_MPU6000_FIFO_REG | 0x80;
for(uint32_t i = 1; i < (1 + mpu6000_last_read_count * sizeof(struct pios_mpu6000_data)); i++)
mpu6000_send_buf[i] = 0;
mpu6000_transfer_size = 1+mpu6000_last_read_count * sizeof(struct pios_mpu6000_data);
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], mpu6000_transfer_size, NULL) < 0) {
PIOS_MPU6000_ReleaseBus();
mpu6000_fails++;
return;
}
PIOS_MPU6000_ReleaseBus();
for(int32_t i = 0; i < mpu6000_last_read_count; i++) {
struct pios_mpu6000_data data;
if(fifoBuf_getFree(&pios_mpu6000_fifo) < sizeof(data)) { if(fifoBuf_getFree(&pios_mpu6000_fifo) < sizeof(data)) {
PIOS_MPU6000_ReleaseBus();
mpu6000_fifo_full++; mpu6000_fifo_full++;
return; return;
} }
uint8_t buf[1+8] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0}; uint32_t start = 1 + i * sizeof(struct pios_mpu6000_data);
uint8_t rec[1+8]; data.temperature = mpu6000_rec_buf[start] << 8 | mpu6000_rec_buf[start+1];
data.gyro_x = mpu6000_rec_buf[start+2] << 8 | mpu6000_rec_buf[start+3];
data.gyro_y = mpu6000_rec_buf[start+4] << 8 | mpu6000_rec_buf[start+5];
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &buf[0], &rec[0], sizeof(buf), NULL) < 0) { data.gyro_z = mpu6000_rec_buf[start+6] << 8 | mpu6000_rec_buf[start+7];
PIOS_MPU6000_ReleaseBus();
return;
}
data.temperature = rec[1] << 8 | rec[2];
data.gyro_x = rec[3] << 8 | rec[4];
data.gyro_y = rec[5] << 8 | rec[6];
data.gyro_z = rec[7] << 8 | rec[8];
fifoBuf_putData(&pios_mpu6000_fifo, (uint8_t *) &data, sizeof(data));
fifoBuf_putData(&pios_mpu6000_fifo, (uint8_t *) &data, sizeof(data));
} }
PIOS_MPU6000_ReleaseBus();
mpu6000_irq++; mpu6000_irq++;
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
} }
#if defined(PIOS_INCLUDE_MPU6000)
/**
* The physical IRQ handler
* Soon this will be generic in pios_exti and the BMA180 will register
* against it. Right now this is crap!
*/
void EXTI1_IRQHandler(void)
{
if (EXTI_GetITStatus(EXTI_Line1) != RESET)
{
PIOS_MPU6000_IRQHandler();
EXTI_ClearITPendingBit(EXTI_Line1);
}
}
#endif
#endif #endif
/** /**

2
flight/PiOS/STM32F4xx/pios_spi.c
View File

@ -43,7 +43,7 @@
#include <pios_spi_priv.h> #include <pios_spi_priv.h>
#define SPI_MAX_BLOCK_PIO 64 #define SPI_MAX_BLOCK_PIO 128
static bool PIOS_SPI_validate(struct pios_spi_dev * com_dev) static bool PIOS_SPI_validate(struct pios_spi_dev * com_dev)
{ {

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

@ -120,9 +120,9 @@ struct pios_mpu6000_data {
int16_t gyro_x; int16_t gyro_x;
int16_t gyro_y; int16_t gyro_y;
int16_t gyro_z; int16_t gyro_z;
int16_t accel_x; /* int16_t accel_x;
int16_t accel_y; int16_t accel_y;
int16_t accel_z; int16_t accel_z; */
int16_t temperature; int16_t temperature;
}; };

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

@ -175,7 +175,7 @@ static const struct pios_spi_cfg pios_spi_gyro_cfg = {
.SPI_CRCPolynomial = 7, .SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High, .SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge, .SPI_CPHA = SPI_CPHA_2Edge,
// .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4, .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2,
}, },
.use_crc = false, .use_crc = false,
.dma = { .dma = {