/**
* @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 Writes one byte to the MPU9250 register
* \param[in] reg Register address
* \param[in] data Byte to write
* @returns 0 when success
*/
static int32_t PIOS_MPU9250_WriteReg(uint8_t reg, uint8_t data)
{
if (PIOS_MPU9250_ClaimBus(true) != 0)
return -1;
PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg);
PIOS_SPI_TransferByte(dev->spi_id, data);
PIOS_MPU9250_ReleaseBus(true);
return 0;
}
/**
* @brief Write a BMA180 register. EEPROM must be unlocked before calling this function.
* @return none
* @param reg[in] address of register to be written
* @param data[in] data that is to be written to register
*/
int32_t PIOS_BMA180_SetReg(uint8_t reg, uint8_t data)
{
if(PIOS_BMA180_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg);
PIOS_SPI_TransferByte(dev->spi_id, data);
PIOS_BMA180_ReleaseBus();
return 0;
}
/**
* @brief Writes one byte to the L3GD20
* \param[in] reg Register address
* \param[in] data Byte to write
* \return 0 if operation was successful
* \return -1 if unable to claim SPI bus
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_L3GD20_SetReg(uint8_t reg, uint8_t data)
{
if (PIOS_L3GD20_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferByte(pios_l3gd20_dev->spi_id, 0x7f & reg);
PIOS_SPI_TransferByte(pios_l3gd20_dev->spi_id, data);
PIOS_L3GD20_ReleaseBus();
return 0;
}
/**
* @brief Read a register from MPU6000
* @returns The register value or -1 if failure to get bus
* @param reg[in] Register address to be read
*/
static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
{
uint8_t data;
if (PIOS_MPU6000_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferByte(pios_mpu6000_dev->spi_id, (0x80 | reg)); // request byte
data = PIOS_SPI_TransferByte(pios_mpu6000_dev->spi_id, 0); // receive response
PIOS_MPU6000_ReleaseBus();
return data;
}
/**
* @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;
}
/**
* @brief Read a register from MPU9250
* @returns The register value
* @param reg[in] Register address to be read
*/
static uint8_t PIOS_MPU9250_ReadReg(uint8_t reg)
{
uint8_t data;
PIOS_MPU9250_ClaimBus(true);
PIOS_SPI_TransferByte(dev->spi_id, 0x80 | reg); // request byte
data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response
PIOS_MPU9250_ReleaseBus(true);
return data;
}
/**
* @brief Read a register from L3GD20 from ISR context
* @returns The register value or -1 if failure to get bus
* @param reg[in] Register address to be read
* \param[in] task woken
*/
static int32_t PIOS_L3GD20_GetRegIsr(uint8_t reg, bool *woken)
{
uint8_t data;
if(PIOS_L3GD20_ClaimBusIsr(woken) != 0)
return -1;
PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte
data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response
PIOS_L3GD20_ReleaseBusIsr(woken);
return data;
}
/**
* @brief Read a register from BMA180
* @returns The register value or -1 if failure to get bus
* @param reg[in] Register address to be read
*/
int32_t PIOS_BMA180_GetReg(uint8_t reg)
{
if(PIOS_BMA180_Validate(dev) != 0)
return -1;
uint8_t data;
if(PIOS_BMA180_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferByte(dev->spi_id,(0x80 | reg) ); // request byte
data = PIOS_SPI_TransferByte(dev->spi_id,0 ); // receive response
PIOS_BMA180_ReleaseBus();
return data;
}
/**
* @brief Writes one byte to the MPU6000
* \param[in] reg Register address
* \param[in] data Byte to write
* \return 0 if operation was successful
* \return -1 if unable to claim SPI bus
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
{
if (PIOS_MPU6000_ClaimBus() != 0)
return -1;
if (PIOS_SPI_TransferByte(pios_mpu6000_dev->spi_id, 0x7f & reg) != 0) {
PIOS_MPU6000_ReleaseBus();
return -2;
}
if (PIOS_SPI_TransferByte(pios_mpu6000_dev->spi_id, data) != 0) {
PIOS_MPU6000_ReleaseBus();
return -3;
}
PIOS_MPU6000_ReleaseBus();
return 0;
}
enum opahrs_result PIOS_OPAHRS_resync(void)
{
struct opahrs_msg_v1 req;
struct opahrs_msg_v1 rsp;
enum opahrs_result rc = OPAHRS_RESULT_FAILED;
opahrs_msg_v1_init_link_tx(&req, OPAHRS_MSG_LINK_TAG_NOP);
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
#ifdef PIOS_INCLUDE_FREERTOS
vTaskDelay(MS2TICKS(1));
#else
PIOS_DELAY_WaitmS(20);
#endif
for (uint32_t i = 0; i < sizeof(req); i++) {
/* Tx a shortened (by one byte) message to walk through all byte positions */
opahrs_msg_v1_init_rx(&rsp);
PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, (uint8_t *) & req, (uint8_t *) & rsp, sizeof(req) - 1, NULL);
/* Good magic means we're sync'd */
if ((rsp.head.magic == OPAHRS_MSG_MAGIC_HEAD) && (rsp.tail.magic == OPAHRS_MSG_MAGIC_TAIL)) {
/* We need to shift out one more byte to compensate for the short tx */
PIOS_SPI_TransferByte(PIOS_OPAHRS_SPI, 0x00);
rc = OPAHRS_RESULT_OK;
break;
}
#ifdef PIOS_INCLUDE_FREERTOS
vTaskDelay(MS2TICKS(1));
#else
PIOS_DELAY_WaitmS(10);
#endif
}
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1);
//vTaskDelay(MS2TICKS(5));
return rc;
}
/**
* @brief Write one page of data (up to 256 bytes) aligned to a page start
* @param[in] addr Address in flash to write to
* @param[in] data Pointer to data to write to flash
* @param[in] len Length of data to write (max 256 bytes)
* @return Zero if success or error code
* @retval -1 Unable to claim SPI bus
* @retval -2 Size exceeds 256 bytes
* @retval -3 Length to write would wrap around page boundary
*/
static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len)
{
struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id;
if(PIOS_Flash_Jedec_Validate(flash_dev) != 0)
return -1;
uint8_t ret;
uint8_t out[4] = {JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff};
/* Can only write one page at a time */
if (len > 0x100)
return -2;
/* Ensure number of bytes fits after starting address before end of page */
if (((addr & 0xff) + len) > 0x100)
return -3;
if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0)
return ret;
/* Execute write page command and clock in address. Keep CS asserted */
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0)
return -1;
if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -1;
}
/* Clock out data to flash */
if (PIOS_SPI_TransferBlock(flash_dev->spi_id,data,NULL,len,NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -1;
}
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
// Keep polling when bus is busy too
#if defined(FLASH_FREERTOS)
while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) {
vTaskDelay(1);
}
#else
// Query status this way to prevent accel chip locking us out
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0)
return -1;
PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS);
while (PIOS_SPI_TransferByte(flash_dev->spi_id, JEDEC_READ_STATUS) & JEDEC_STATUS_BUSY);
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
#endif
return 0;
}
/**
* @brief Write multiple chunks of data in one transaction
* @param[in] addr Address in flash to write to
* @param[in] data Pointer to data to write to flash
* @param[in] len Length of data to write (max 256 bytes)
* @return Zero if success or error code
* @retval -1 Unable to claim SPI bus
* @retval -2 Size exceeds 256 bytes
* @retval -3 Length to write would wrap around page boundary
*/
static int32_t PIOS_Flash_Jedec_WriteChunks(uintptr_t flash_id, uint32_t addr, struct pios_flash_chunk chunks[], uint32_t num)
{
struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id;
if (PIOS_Flash_Jedec_Validate(flash_dev) != 0)
return -1;
uint8_t ret;
uint8_t out[4] = {JEDEC_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff};
/* Can only write one page at a time */
uint32_t len = 0;
for (uint32_t i = 0; i < num; i++)
len += chunks[i].len;
if (len > 0x100)
return -2;
/* Ensure number of bytes fits after starting address before end of page */
if (((addr & 0xff) + len) > 0x100)
return -3;
if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0)
return ret;
/* Execute write page command and clock in address. Keep CS asserted */
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0)
return -1;
if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -1;
}
for (uint32_t i = 0; i < num; i++) {
struct pios_flash_chunk * chunk = &chunks[i];
/* Clock out data to flash */
if (PIOS_SPI_TransferBlock(flash_dev->spi_id,chunk->addr,NULL,chunk->len,NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -1;
}
}
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
// Skip checking for busy with this to get OS running again fast
return 0;
}
/**
* @brief Read data from a location in flash memory
* @param[in] addr Address in flash to write to
* @param[in] data Pointer to data to write from flash
* @param[in] len Length of data to write (max 256 bytes)
* @return Zero if success or error code
* @retval -1 Unable to claim SPI bus
*/
static int32_t PIOS_Flash_Jedec_ReadData(uintptr_t flash_id, uint32_t addr, uint8_t * data, uint16_t len)
{
struct jedec_flash_dev * flash_dev = (struct jedec_flash_dev *)flash_id;
if (PIOS_Flash_Jedec_Validate(flash_dev) != 0)
return -1;
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) == -1)
return -1;
/* Execute read command and clock in address. Keep CS asserted */
uint8_t out[] = {JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff};
if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -2;
}
/* Copy the transfer data to the buffer */
if (PIOS_SPI_TransferBlock(flash_dev->spi_id,NULL,data,len,NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -3;
}
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return 0;
}
/* Provide a flash driver to external drivers */
const struct pios_flash_driver pios_jedec_flash_driver = {
.start_transaction = PIOS_Flash_Jedec_StartTransaction,
.end_transaction = PIOS_Flash_Jedec_EndTransaction,
.erase_chip = PIOS_Flash_Jedec_EraseChip,
.erase_sector = PIOS_Flash_Jedec_EraseSector,
.write_chunks = PIOS_Flash_Jedec_WriteChunks,
.write_data = PIOS_Flash_Jedec_WriteData,
.read_data = PIOS_Flash_Jedec_ReadData,
};
