/** * @brief Obtains the number of bytes in the FIFO. Call from ISR only. * @return the number of bytes in the FIFO * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority * task has is now eligible to run, else unchanged */ static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken) { uint8_t mpu6000_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 }; uint8_t mpu6000_rec_buf[3]; if (PIOS_MPU6000_ClaimBusISR(woken) != 0) { return -1; } if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { PIOS_MPU6000_ReleaseBusISR(woken); return -1; } PIOS_MPU6000_ReleaseBusISR(woken); return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2]; }
/** * @brief Reads the contents of the MPU6000 Interrupt Status register from an ISR * @return The register value or -1 on failure to claim the bus */ static int32_t PIOS_MPU6000_GetInterruptStatusRegISR(bool *woken) { /* Interrupt Status register can be read at high SPI clock speed */ uint8_t data; if (PIOS_MPU6000_ClaimBusISR(woken) != 0) { return -1; } PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG)); data = PIOS_SPI_TransferByte(dev->spi_id, 0); PIOS_MPU6000_ReleaseBusISR(woken); return data; }
bool PIOS_MPU6000_IRQHandler(void) { bool woken = false; static uint32_t timeval; mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval); timeval = PIOS_DELAY_GetRaw(); if (!mpu6000_configured) { return false; } /* Temporary fix for OP-1049. Expected to be superceded for next major release * by code changes for OP-1039. * Read interrupt status register to check for FIFO overflow. Must be the * first read after interrupt, in case the device is configured so that * any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in * interrupt config register) */ int32_t result; if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(&woken)) < 0) { return woken; } if (result & PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW) { /* The FIFO has overflowed, so reset it, * to enable sample sync to be recovered. * If the reset fails, we are in trouble, but * we keep trying on subsequent interrupts. */ PIOS_MPU6000_ResetFifoISR(&woken); /* Return and wait for the next new sample. */ return woken; } /* Usual case - FIFO has not overflowed. */ mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken); if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) { return woken; } if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { return woken; } static uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 }; static uint8_t mpu6000_rec_buf[1 + PIOS_MPU6000_SAMPLES_BYTES]; if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { PIOS_MPU6000_ReleaseBusISR(&woken); mpu6000_fails++; return woken; } PIOS_MPU6000_ReleaseBusISR(&woken); static struct pios_mpu6000_data data; // In the case where extras samples backed up grabbed an extra if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) { mpu6000_fifo_backup++; if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { return woken; } if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { PIOS_MPU6000_ReleaseBusISR(&woken); mpu6000_fails++; return woken; } PIOS_MPU6000_ReleaseBusISR(&woken); } // Rotate the sensor to OP convention. The datasheet defines X as towards the right // and Y as forward. OP convention transposes this. Also the Z is defined negatively // to our convention #if defined(PIOS_MPU6000_ACCEL) // Currently we only support rotations on top so switch X/Y accordingly switch (dev->cfg->orientation) { case PIOS_MPU6000_TOP_0DEG: data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y data.gyro_y = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X data.gyro_x = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y break; case PIOS_MPU6000_TOP_90DEG: // -1 to bring it back to -32768 +32767 range data.accel_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X data.gyro_y = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X break; case PIOS_MPU6000_TOP_180DEG: data.accel_y = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X data.accel_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y data.gyro_y = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X data.gyro_x = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y break; case PIOS_MPU6000_TOP_270DEG: data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y data.accel_x = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y data.gyro_x = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X break; } data.gyro_z = -1 - (mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]); data.accel_z = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]; #else /* if defined(PIOS_MPU6000_ACCEL) */ data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; switch (dev->cfg->orientation) { case PIOS_MPU6000_TOP_0DEG: data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; break; case PIOS_MPU6000_TOP_90DEG: data.gyro_y = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X break; case PIOS_MPU6000_TOP_180DEG: data.gyro_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); data.gyro_x = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); break; case PIOS_MPU6000_TOP_270DEG: data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y data.gyro_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X break; } data.gyro_z = -1 - (mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]); data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; #endif /* if defined(PIOS_MPU6000_ACCEL) */ signed portBASE_TYPE higherPriorityTaskWoken; xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken); mpu6000_irq++; mpu6000_time_us = PIOS_DELAY_DiffuS(timeval); return woken || higherPriorityTaskWoken == pdTRUE; }
/** * @brief IRQ Handler. Read all the data from onboard buffer */ bool PIOS_MPU6000_IRQHandler(void) { if (PIOS_MPU6000_Validate(pios_mpu6000_dev) != 0 || pios_mpu6000_dev->configured == false) return false; bool woken = false; if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) return false; enum { IDX_SPI_DUMMY_BYTE = 0, IDX_ACCEL_XOUT_H, IDX_ACCEL_XOUT_L, IDX_ACCEL_YOUT_H, IDX_ACCEL_YOUT_L, IDX_ACCEL_ZOUT_H, IDX_ACCEL_ZOUT_L, IDX_TEMP_OUT_H, IDX_TEMP_OUT_L, IDX_GYRO_XOUT_H, IDX_GYRO_XOUT_L, IDX_GYRO_YOUT_H, IDX_GYRO_YOUT_L, IDX_GYRO_ZOUT_H, IDX_GYRO_ZOUT_L, BUFFER_SIZE, }; uint8_t mpu6000_send_buf[BUFFER_SIZE] = { PIOS_MPU60X0_ACCEL_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; uint8_t mpu6000_rec_buf[BUFFER_SIZE]; if (PIOS_SPI_TransferBlock(pios_mpu6000_dev->spi_id, mpu6000_send_buf, mpu6000_rec_buf, sizeof(mpu6000_send_buf), NULL) < 0) { PIOS_MPU6000_ReleaseBusISR(&woken); return false; } PIOS_MPU6000_ReleaseBusISR(&woken); // Rotate the sensor to OP convention. The datasheet defines X as towards the right // and Y as forward. OP convention transposes this. Also the Z is defined negatively // to our convention #if defined(PIOS_MPU6000_ACCEL) // Currently we only support rotations on top so switch X/Y accordingly struct pios_sensor_accel_data accel_data; struct pios_sensor_gyro_data gyro_data; switch (pios_mpu6000_dev->cfg->orientation) { case PIOS_MPU60X0_TOP_0DEG: accel_data.y = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]); accel_data.x = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]); gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]); accel_data.z = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]); break; case PIOS_MPU60X0_TOP_90DEG: accel_data.y = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]); accel_data.x = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]); gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]); accel_data.z = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]); break; case PIOS_MPU60X0_TOP_180DEG: accel_data.y = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]); accel_data.x = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]); gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]); accel_data.z = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]); break; case PIOS_MPU60X0_TOP_270DEG: accel_data.y = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]); accel_data.x = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]); gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]); accel_data.z = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]); break; case PIOS_MPU60X0_BOTTOM_0DEG: accel_data.y = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]); accel_data.x = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]); gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.z = (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]); accel_data.z = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]); break; case PIOS_MPU60X0_BOTTOM_90DEG: accel_data.y = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]); accel_data.x = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]); gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.z = (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]); accel_data.z = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]); break; case PIOS_MPU60X0_BOTTOM_180DEG: accel_data.y = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]); accel_data.x = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]); gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.z = (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]); accel_data.z = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]); break; case PIOS_MPU60X0_BOTTOM_270DEG: accel_data.y = - (int16_t)(mpu6000_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_YOUT_L]); accel_data.x = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_XOUT_L]); gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.z = (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]); accel_data.z = (int16_t)(mpu6000_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_ACCEL_ZOUT_L]); break; } int16_t raw_temp = (int16_t)(mpu6000_rec_buf[IDX_TEMP_OUT_H] << 8 | mpu6000_rec_buf[IDX_TEMP_OUT_L]); float temperature = 35.0f + ((float)raw_temp + 512.0f) / 340.0f; // Apply sensor scaling float accel_scale = PIOS_MPU6000_GetAccelScale(); accel_data.x *= accel_scale; accel_data.y *= accel_scale; accel_data.z *= accel_scale; accel_data.temperature = temperature; float gyro_scale = PIOS_MPU6000_GetGyroScale(); gyro_data.x *= gyro_scale; gyro_data.y *= gyro_scale; gyro_data.z *= gyro_scale; gyro_data.temperature = temperature; PIOS_Queue_Send_FromISR(pios_mpu6000_dev->accel_queue, &accel_data, &woken); PIOS_Queue_Send_FromISR(pios_mpu6000_dev->gyro_queue, &gyro_data, &woken); return woken; #else struct pios_sensor_gyro_data gyro_data; switch (pios_mpu6000_dev->cfg->orientation) { case PIOS_MPU60X0_TOP_0DEG: gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); break; case PIOS_MPU60X0_TOP_90DEG: gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.x = (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); break; case PIOS_MPU60X0_TOP_180DEG: gyro_data.y = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); break; case PIOS_MPU60X0_TOP_270DEG: gyro_data.y = (int16_t)(mpu6000_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_YOUT_L]); gyro_data.x = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_XOUT_L]); break; } gyro_data.z = - (int16_t)(mpu6000_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000_rec_buf[IDX_GYRO_ZOUT_L]); int32_t raw_temp = (int16_t)(mpu6000_rec_buf[IDX_TEMP_OUT_H] << 8 | mpu6000_rec_buf[IDX_TEMP_OUT_L]); float temperature = 35.0f + ((float)raw_temp + 512.0f) / 340.0f; // Apply sensor scaling float gyro_scale = PIOS_MPU6000_GetGyroScale(); gyro_data.x *= gyro_scale; gyro_data.y *= gyro_scale; gyro_data.z *= gyro_scale; gyro_data.temperature = temperature; PIOS_Queue_Send_FromISR(pios_mpu6000_dev->gyro_queue, &gyro_data, &woken); return woken; #endif /* PIOS_MPU6000_ACCEL */ }