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

OP-911: First part of the rework: Fixed non isr call for claimbus, changed delays at startup to resolve a intermittent failure at MPU6000 initialization on reboot.

Browse Source 
+review OPReview
This commit is contained in:
Alessio Morale 2013-04-17 19:22:56 +02:00
parent 61bb82ca15
commit 6214d0d471
2 changed files with 19 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
flight/PiOS/Common/pios_mpu6000.c
View File

@ -141,7 +141,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
// Reset chip
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0);
PIOS_DELAY_WaitmS(100);
PIOS_DELAY_WaitmS(50);
// Reset chip and fifo
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG,
@ -154,7 +154,7 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg)
(PIOS_MPU6000_USERCTL_GYRO_RST |
PIOS_MPU6000_USERCTL_SIG_COND |
PIOS_MPU6000_USERCTL_FIFO_RST));
PIOS_DELAY_WaitmS(10);
//Power management configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0);
@ -228,14 +228,17 @@ int32_t PIOS_MPU6000_ConfigureRanges(
* @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/
int32_t PIOS_MPU6000_ClaimBus()
static int32_t PIOS_MPU6000_ClaimBus(bool fromIsr)
{
if(PIOS_MPU6000_Validate(dev) != 0)
return -1;
if(PIOS_SPI_ClaimBus(dev->spi_id) != 0)
return -2;
if(fromIsr){
if(PIOS_SPI_ClaimBusISR(dev->spi_id) != 0)
return -2;
} else {
if(PIOS_SPI_ClaimBus(dev->spi_id) != 0)
return -2;
}
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
return 0;
}
@ -263,7 +266,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
{
uint8_t data;
if(PIOS_MPU6000_ClaimBus() != 0)
if(PIOS_MPU6000_ClaimBus(false) != 0)
return -1;
PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte
@ -283,7 +286,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
*/
static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
{
if(PIOS_MPU6000_ClaimBus() != 0)
if(PIOS_MPU6000_ClaimBus(false) != 0)
return -1;
if(PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg) != 0) {
@ -312,7 +315,7 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data * data)
uint8_t buf[7] = {PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0};
uint8_t rec[7];
if(PIOS_MPU6000_ClaimBus() != 0)
if(PIOS_MPU6000_ClaimBus(false) != 0)
return -1;
if(PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0)
@ -405,12 +408,12 @@ int32_t PIOS_MPU6000_Test(void)
* \return 0 if test succeeded
* \return non-zero value if test succeeded
*/
static int32_t PIOS_MPU6000_FifoDepth(void)
static int32_t PIOS_MPU6000_FifoDepth(bool fromIsr)
{
uint8_t mpu6000_send_buf[3] = {PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0};
uint8_t mpu6000_rec_buf[3];
if(PIOS_MPU6000_ClaimBus() != 0)
if(PIOS_MPU6000_ClaimBus(fromIsr) != 0)
return -1;
if(PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
@ -446,11 +449,11 @@ bool PIOS_MPU6000_IRQHandler(void)
if (!mpu6000_configured)
return false;
mpu6000_count = PIOS_MPU6000_FifoDepth();
mpu6000_count = PIOS_MPU6000_FifoDepth(true);
if (mpu6000_count < sizeof(struct pios_mpu6000_data))
return false;
if (PIOS_MPU6000_ClaimBus() != 0)
if (PIOS_MPU6000_ClaimBus(true) != 0)
return false;
uint8_t mpu6000_send_buf[1 + sizeof(struct pios_mpu6000_data) ] = {PIOS_MPU6000_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
@ -469,7 +472,7 @@ bool PIOS_MPU6000_IRQHandler(void)
// In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= (sizeof(data) * 2)) {
mpu6000_fifo_backup++;
if (PIOS_MPU6000_ClaimBus() != 0)
if (PIOS_MPU6000_ClaimBus(true) != 0)
return false;
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {

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

@ -275,7 +275,7 @@ int32_t PIOS_SPI_ClaimBusISR(uint32_t spi_id)
bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid)
if (xQueueGenericReceive(( xQueueHandle ) spi_dev->busy, NULL, 0x0000 , pdFALSE ) != pdTRUE)
if (xSemaphoreTakeFromISR(( xQueueHandle ) spi_dev->busy, NULL) != pdTRUE)
return -1;
#endif
return 0;