/**
* @brief Resets the MPU6000 FIFO from an ISR
* @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
* @return 0 if operation was successful
* @return -1 if unable to claim SPI bus
* @return -2 if write to the device failed
*/
static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken)
{
int32_t result = 0;
/* Register writes must be at < 1MHz SPI clock.
* Speed can only be changed when SPI bus semaphore is held, but
* device chip select must not be enabled, so we use the direct
* SPI bus claim call here */
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -1;
}
/* Reduce SPI clock speed. */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
/* Enable chip select */
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
/* Reset FIFO. */
if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) {
result = -2;
} else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) {
result = -2;
}
/* Disable chip select. */
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
/* Increase SPI clock speed. */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
/* Release the SPI bus semaphore. */
PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
return result;
}
/**
* @brief Initialize the MPU6000 3-axis gyro sensor.
* @return 0 for success, -1 for failure
*/
int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu60x0_cfg *cfg)
{
pios_mpu6000_dev = PIOS_MPU6000_alloc();
if (pios_mpu6000_dev == NULL)
return -1;
pios_mpu6000_dev->spi_id = spi_id;
pios_mpu6000_dev->slave_num = slave_num;
pios_mpu6000_dev->cfg = cfg;
/* Configure the MPU6000 Sensor */
PIOS_SPI_SetClockSpeed(pios_mpu6000_dev->spi_id, 100000);
PIOS_MPU6000_Config(cfg);
PIOS_SPI_SetClockSpeed(pios_mpu6000_dev->spi_id, 3000000);
pios_mpu6000_dev->threadp = PIOS_Thread_Create(
PIOS_MPU6000_Task, "pios_mpu6000", MPU6000_TASK_STACK, NULL, MPU6000_TASK_PRIORITY);
PIOS_Assert(pios_mpu6000_dev->threadp != NULL);
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
#if defined(PIOS_MPU6000_ACCEL)
PIOS_SENSORS_Register(PIOS_SENSOR_ACCEL, pios_mpu6000_dev->accel_queue);
#endif /* PIOS_MPU6000_ACCEL */
PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, pios_mpu6000_dev->gyro_queue);
return 0;
}
void AhrsInitComms(void)
{
programReceive = false;
AhrsInitHandles();
memset(objCallbacks, 0, sizeof(AhrsEventCallback) * MAX_AHRS_OBJECTS);
memset(callbackPending, 0, sizeof(bool) * MAX_AHRS_OBJECTS);
memset(dirtyObjects, 0, sizeof(unsigned int) * MAX_AHRS_OBJECTS);
memset(&txPacket, 0, sizeof(txPacket));
memset(&rxPacket, 0, sizeof(rxPacket));
memset(&readyObjects, 0, sizeof(bool) * MAX_AHRS_OBJECTS);
txPacket.command = COMMS_NULL;
rxPacket.command = COMMS_NULL;
#ifdef IN_AHRS
PIOS_SPI_Init();
#else
PIOS_SPI_SetClockSpeed(PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_8); //36MHz/16 = 2.25MHz
for (int ct = 0; ct < MAX_AHRS_OBJECTS; ct++) {
AhrsObjHandle hdl = AhrsFromIndex(ct);
if (hdl) {
AhrsConnectCallBack(hdl, AhrsUpdatedCb);
}
}
#endif
}
/**
* @brief Configures Gyro, accel and Filter ranges/setings
* @return 0 if successful, -1 if device has not been initialized
*/
int32_t PIOS_MPU6000_ConfigureRanges(
enum pios_mpu6000_range gyroRange,
enum pios_mpu6000_accel_range accelRange,
enum pios_mpu6000_filter filterSetting)
{
if (dev == NULL) {
return -1;
}
// TODO: check that changing the SPI clock speed is safe
// to do here given that interrupts are enabled and the bus has
// not been claimed/is not claimed during this call
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
// update filter settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) {
;
}
// Sample rate divider, chosen upon digital filtering settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_SMPLRT_DIV_REG,
filterSetting == PIOS_MPU6000_LOWPASS_256_HZ ?
dev->cfg->Smpl_rate_div_no_dlp : dev->cfg->Smpl_rate_div_dlp) != 0) {
;
}
dev->filter = filterSetting;
// Gyro range
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_GYRO_CFG_REG, gyroRange) != 0) {
;
}
dev->gyro_range = gyroRange;
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel range
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) {
;
}
dev->accel_range = accelRange;
#endif
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
return 0;
}
/**
* @brief Initialize the MPU6000 3-axis gyro sensor.
* @return 0 for success, -1 for failure
*/
int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg)
{
dev = PIOS_MPU6000_alloc();
if (dev == NULL) {
return -1;
}
dev->spi_id = spi_id;
dev->slave_num = slave_num;
dev->cfg = cfg;
/* Configure the MPU6000 Sensor */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
PIOS_MPU6000_Config(cfg);
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
return 0;
}
/**
* @brief Claim the SPI bus for flash use and assert CS pin
* @return 0 for sucess, -1 for failure to get semaphore
*/
static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev, bool fast)
{
if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0) {
return -1;
}
PIOS_SPI_SetClockSpeed(flash_dev->spi_id, fast ? FLASH_FAST_PRESCALER : FLASH_PRESCALER);
PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0);
flash_dev->claimed = true;
return 0;
}
/**
* @brief Release the SPI bus for the communications and end the transaction
* \param[in] must be true when bus was claimed in lowspeed mode
* @return 0 if successful
*/
static int32_t PIOS_MPU9250_ReleaseBus(bool lowspeed)
{
if (PIOS_MPU9250_Validate(dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
if (lowspeed)
PIOS_SPI_SetClockSpeed(dev->spi_id, MPU9250_SPI_HIGH_SPEED);
PIOS_SPI_ReleaseBus(dev->spi_id);
return 0;
}
/**
* @brief Claim the SPI bus for the communications and select this chip
* \param[in] flag controls if low speed access for control registers should be used
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/
static int32_t PIOS_MPU9250_ClaimBus(bool lowspeed)
{
if (PIOS_MPU9250_Validate(dev) != 0)
return -1;
if (PIOS_SPI_ClaimBus(dev->spi_id) != 0)
return -2;
if (lowspeed)
PIOS_SPI_SetClockSpeed(dev->spi_id, MPU9250_SPI_LOW_SPEED);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
/**
* Initialise the OpenPilot AHRS
*/
void PIOS_OPAHRS_Init(void)
{
PIOS_SPI_SetClockSpeed(PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_8);
}
