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

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
*/
#define MPU6000_MAX_BLOCK_READ 12
uint32_t mpu6000_irq = 0;
uint32_t mpu6000_count;
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)
{
if(!mpu6000_configured)
return;
struct pios_mpu6000_data data;
mpu6000_irq++;
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
uint8_t buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0};
uint8_t rec[3];
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBus();
mpu6000_send_buf[0] = PIOS_MPU6000_FIFO_CNT_MSB | 0x80;
mpu6000_send_buf[1] = mpu6000_send_buf[2] = 0;
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], 3, NULL) < 0) {
PIOS_MPU6000_ReleaseBus();
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)) {
PIOS_MPU6000_ReleaseBus();
mpu6000_fifo_full++;
return;
}
uint8_t buf[1+8] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t rec[1+8];
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
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));
uint32_t start = 1 + i * sizeof(struct pios_mpu6000_data);
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];
data.gyro_z = mpu6000_rec_buf[start+6] << 8 | mpu6000_rec_buf[start+7];
fifoBuf_putData(&pios_mpu6000_fifo, (uint8_t *) &data, sizeof(data));
}
PIOS_MPU6000_ReleaseBus();
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
/**

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

@ -43,7 +43,7 @@
#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)
{

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

@ -120,9 +120,9 @@ struct pios_mpu6000_data {
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
int16_t accel_x;
/* int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t accel_z; */
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_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
// .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2,
},
.use_crc = false,
.dma = {