/**
* @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
*/
int8_t PIOS_Flash_W25X_WriteData(uint32_t addr, uint8_t * data, uint16_t len)
{
uint8_t ret;
uint8_t out[4] = {W25X_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_W25X_WriteEnable()) != 0)
return ret;
/* Execute write page command and clock in address. Keep CS asserted */
if(PIOS_Flash_W25X_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
/* Clock out data to flash */
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,data,NULL,len,NULL);
PIOS_Flash_W25X_ReleaseBus();
while(PIOS_Flash_W25X_Busy()) {
#if defined(PIOS_INCLUDE_FREERTOS)
vTaskDelay(1);
#endif
}
return 0;
}
int8_t PIOS_Flash_W25X_ReadData(uint32_t addr, uint8_t * data, uint16_t len)
{
if(PIOS_Flash_W25X_ClaimBus() == -1)
return -1;
/* Execute read command and clock in address. Keep CS asserted */
uint8_t out[] = {W25X_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff};
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
/* Copy the transfer data to the buffer */
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,NULL,data,len,NULL);
PIOS_Flash_W25X_ReleaseBus();
return 0;
}
PROGERR TransferPacket(uint32_t spi_id, AhrsProgramPacket *txBuf, AhrsProgramPacket *rxBuf)
{
static uint32_t pktId = 0;
pktId++;
txBuf->packetId = pktId;
txBuf->crc = GenerateCRC(txBuf);
int ct = 0;
for(; ct < MAX_CONNECT_TRIES; ct++) {
PIOS_SPI_RC_PinSet(spi_id, 0);
uint32_t res = PIOS_SPI_TransferBlock(spi_id, (uint8_t *) txBuf,
(uint8_t *) rxBuf, sizeof(AhrsProgramPacket), NULL);
PIOS_SPI_RC_PinSet(spi_id, 1);
if(res == 0) {
if(rxBuf->type != PROGRAM_NULL &&
rxBuf->crc == GenerateCRC(rxBuf) &&
rxBuf->packetId == pktId) {
break;
}
}
vTaskDelay(1 / portTICK_RATE_MS);
}
if(ct == MAX_CONNECT_TRIES) {
return (PROGRAM_ERR_LINK);
}
if(rxBuf->type != PROGRAM_ACK) {
return(PROGRAM_ERR_FUNCTION);
}
return(PROGRAM_ERR_OK);
}
/**
* @brief Read a single set of values from the x y z channels
* @returns The number of samples remaining in the fifo
*/
uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data)
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;
if(PIOS_ADXL345_ClaimBus() != 0)
return -2;
// To save memory use same buffer for in and out but offset by
// a byte
uint8_t buf[9] = {0,0,0,0,0,0,0,0};
uint8_t rec[9] = {0,0,0,0,0,0,0,0};
buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT ; // Multibyte read starting at X0
if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],9,NULL) < 0) {
PIOS_ADXL345_ReleaseBus();
return -3;
}
PIOS_ADXL345_ReleaseBus();
data->x = rec[1] + (rec[2] << 8);
data->y = rec[3] + (rec[4] << 8);
data->z = rec[5] + (rec[6] << 8);
return rec[8] & 0x7F; // return number of remaining entries
}
/**
* @brief Execute the whole chip
* @returns 0 if successful, -1 if unable to claim bus
*/
int32_t PIOS_Flash_Jedec_EraseChip()
{
if(PIOS_Flash_Jedec_Validate(flash_dev) != 0)
return -1;
uint8_t ret;
uint8_t out[] = {flash_dev->cfg->chip_erase};
if((ret = PIOS_Flash_Jedec_WriteEnable()) != 0)
return ret;
if(PIOS_Flash_Jedec_ClaimBus() != 0)
return -1;
if(PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus();
return -2;
}
PIOS_Flash_Jedec_ReleaseBus();
// Keep polling when bus is busy too
int i = 0;
while(PIOS_Flash_Jedec_Busy() != 0) {
#if defined(FLASH_FREERTOS)
vTaskDelay(1);
#endif
if ((i++) % 10000 == 0)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
}
return 0;
}
/**
* @brief Test SPI and chip functionality by reading chip ID register
* @return 0 if success, -1 if failure.
*
*/
int32_t PIOS_BMA180_Test()
{
// Read chip ID then version ID
uint8_t buf[3] = {0x80 | BMA_CHIPID_ADDR, 0, 0};
uint8_t rec[3] = {0,0, 0};
int32_t retval;
if(PIOS_BMA180_ClaimBus() != 0)
return -1;
retval = PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL);
PIOS_BMA180_ReleaseBus();
if(retval != 0)
return -2;
struct pios_bma180_data data;
if(PIOS_BMA180_ReadAccels(&data) != 0)
return -3;
if(rec[1] != 0x3)
return -4;
if(rec[2] < 0x12)
return -5;
return 0;
}
/**
* @brief Erase a sector on the flash chip
* @param[in] add Address of flash to erase
* @returns 0 if successful
* @retval -1 if unable to claim bus
* @retval
*/
static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr)
{
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[] = {flash_dev->cfg->sector_erase, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff};
if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0)
return ret;
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 -2;
}
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
// Keep polling when bus is busy too
while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) {
#if defined(FLASH_FREERTOS)
vTaskDelay(1);
#endif
}
return 0;
}
/**
* @brief Execute the write enable instruction and returns the status
* @returns 0 if successful, -1 if unable to claim bus
*/
static uint8_t PIOS_Flash_W25X_WriteEnable()
{
uint8_t out[] = {W25X_WRITE_ENABLE};
if(PIOS_Flash_W25X_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
return 0;
}
/**
* @brief Read the status register from flash chip and return it
*/
uint8_t PIOS_Flash_W25X_ReadStatus()
{
uint8_t out[2] = {W25X_READ_STATUS, 0};
uint8_t in[2] = {0,0};
PIOS_Flash_W25X_ClaimBus();
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,in,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
return in[1];
}
/**
* @brief Read the status register from flash chip and return it
*/
uint8_t PIOS_Flash_W25X_ReadID()
{
uint8_t out[] = {W25X_DEVICE_ID, 0, 0, 0, 0, 0};
uint8_t in[6];
PIOS_Flash_W25X_ClaimBus();
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,in,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
return in[5];
}
/**
* @brief Execute the write enable instruction and returns the status
* @returns 0 if successful, -1 if unable to claim bus
*/
static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev * flash_dev)
{
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0)
return -1;
uint8_t out[] = {JEDEC_WRITE_ENABLE};
PIOS_SPI_TransferBlock(flash_dev->spi_id,out,NULL,sizeof(out),NULL);
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return 0;
}
static int32_t opahrs_msg_txrx(const uint8_t * tx, uint8_t * rx, uint32_t len)
{
int32_t rc;
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
#ifdef PIOS_INCLUDE_FREERTOS
vTaskDelay(MS2TICKS(1));
#else
PIOS_DELAY_WaitmS(20);
#endif
rc = PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, tx, rx, len, NULL);
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1);
return (rc);
}
void CommsCallback(uint8_t crc_ok, uint8_t crc_val)
{
#ifndef IN_AHRS
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1); //signal the end of the transfer
#endif
txPacket.command = COMMS_NULL; //we must send something so default to null
if (rxPacket.magicNumber != RXMAGIC) {
crc_ok = false;
}
if (crc_ok) {
if (!linkOk && okCount > 0) {
okCount--;
if (okCount == 0) {
linkOk = true;
okCount = MAX_CRC_ERRORS;
emptyCount = MIN_EMPTY_OBJECTS;
}
}
HandleRxPacket();
} else {
#ifdef IN_AHRS //AHRS - do we neeed to enter program mode?
if (memcmp(&rxPacket, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH) == 0)
{
rxPacket.magicNumber = 0;
programReceive = true; //flag it to be executed in program space
return;
}
#endif
txPacket.status.crcErrors++;
if (linkOk && okCount > 0) {
okCount--;
if (okCount == 0) {
linkOk = false;
okCount = MIN_OK_FRAMES;
}
}
}
rxPacket.magicNumber = 0;
#ifdef IN_AHRS
/*queue next frame
If PIOS_SPI_TransferBlock() fails for any reason, comms will stop working.
In that case, AhrsPoll() should kick start things again.
*/
PIOS_SPI_TransferBlock(PIOS_SPI_OP, (uint8_t *) & txPacket, (uint8_t *) & rxPacket, sizeof(CommsDataPacket), &CommsCallback);
#endif
}
void AhrsPoll()
{
if(programReceive)
{
AhrsProgramReceive();
programReceive = false;
}
PollEvents();
if (PIOS_SPI_Busy(PIOS_SPI_OP) != 0) { //Everything is working correctly
return;
}
txPacket.status.kickStarts++;
//comms have broken down - try kick starting it.
txPacket.command = COMMS_NULL; //we must send something so default to null
PIOS_SPI_TransferBlock(PIOS_SPI_OP, (uint8_t *) & txPacket, (uint8_t *) & rxPacket, sizeof(CommsDataPacket), &CommsCallback);
}
/**
* @brief Read the status register from flash chip and return it
*/
static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev * flash_dev)
{
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0)
return -1;
uint8_t out[2] = {JEDEC_READ_STATUS, 0};
uint8_t in[2] = {0,0};
if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,in,sizeof(out),NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -2;
}
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return in[1];
}
/**
* @brief Read the status register from flash chip and return it
*/
int32_t PIOS_Flash_Jedec_ReadID()
{
uint8_t out[] = {JEDEC_DEVICE_ID, 0, 0, 0};
uint8_t in[4];
if (PIOS_Flash_Jedec_ClaimBus() < 0)
return -1;
if(PIOS_SPI_TransferBlock(flash_dev->spi_id,out,in,sizeof(out),NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus();
return -2;
}
PIOS_Flash_Jedec_ReleaseBus();
flash_dev->device_type = in[1];
flash_dev->capacity = in[3];
return in[1];
}
/**
* @brief Enable measuring. This also disables the activity sensors (tap or free fall)
*/
static int32_t PIOS_ADXL345_SetMeasure(uint8_t enable)
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;
if(PIOS_ADXL345_ClaimBus() != 0)
return -2;
uint8_t out[2] = {ADXL_POWER_ADDR, ADXL_MEAURE};
if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) {
PIOS_ADXL345_ReleaseBus();
return -3;
}
PIOS_ADXL345_ReleaseBus();
return 0;
}
/**
* @brief Set the range of the accelerometer and set the data to be right justified
* with sign extension. Also keep device in 4 wire mode.
*/
int32_t PIOS_ADXL345_SetRange(uint8_t range)
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;
if(PIOS_ADXL345_ClaimBus() != 0)
return -2;
uint8_t out[2] = {ADXL_FORMAT_ADDR, (range & 0x03) | ADXL_FULL_RES | ADXL_4WIRE};
if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) {
PIOS_ADXL345_ReleaseBus();
return -3;
}
PIOS_ADXL345_ReleaseBus();
return 0;
}
/**
* @brief Select the sampling rate of the chip
*
* This also puts it into high power mode
*/
int32_t PIOS_ADXL345_SelectRate(uint8_t rate)
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;
if(PIOS_ADXL345_ClaimBus() != 0)
return -2;
uint8_t out[2] = {ADXL_RATE_ADDR, rate & 0x0F};
if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) {
PIOS_ADXL345_ReleaseBus();
return -3;
}
PIOS_ADXL345_ReleaseBus();
return 0;
}
/**
* @brief Set the fifo depth that triggers an interrupt. This will be matched to the oversampling
*/
static int32_t PIOS_ADXL345_FifoDepth(uint8_t depth)
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;
if(PIOS_ADXL345_ClaimBus() != 0)
return -2;
uint8_t out[2] = {ADXL_FIFO_ADDR, (depth & 0x1f) | ADXL_FIFO_STREAM};
if(PIOS_SPI_TransferBlock(dev->spi_id,out,NULL,sizeof(out),NULL) < 0) {
PIOS_ADXL345_ReleaseBus();
return -3;
}
PIOS_ADXL345_ReleaseBus();
return 0;
}
/**
* @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];
}
bool AhrsProgramConnect(uint32_t spi_id)
{
AhrsProgramPacket rxBuf;
AhrsProgramPacket txBuf;
memset(&rxBuf, 0, sizeof(AhrsProgramPacket));
memcpy(&txBuf,SPI_PROGRAM_REQUEST,SPI_PROGRAM_REQUEST_LENGTH);
for(int ct = 0; ct < MAX_CONNECT_TRIES; ct++) {
PIOS_SPI_RC_PinSet(spi_id, 0);
uint32_t res = PIOS_SPI_TransferBlock(spi_id, (uint8_t *) &txBuf,
(uint8_t *) & rxBuf, SPI_PROGRAM_REQUEST_LENGTH +1, NULL);
PIOS_SPI_RC_PinSet(spi_id, 1);
if(res == 0 && memcmp(&rxBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH) == 0) {
return (true);
}
vTaskDelay(1 / portTICK_RATE_MS);
}
return (false);
}
/**
* @brief Read the status register from flash chip and return it
*/
static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev * flash_dev)
{
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0)
return -2;
uint8_t out[] = {JEDEC_DEVICE_ID, 0, 0, 0};
uint8_t in[4];
if (PIOS_SPI_TransferBlock(flash_dev->spi_id,out,in,sizeof(out),NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -3;
}
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
flash_dev->manufacturer = in[1];
flash_dev->memorytype = in[2];
flash_dev->capacity = in[3];
return flash_dev->manufacturer;
}
/**
* @brief Execute the whole chip
* @returns 0 if successful, -1 if unable to claim bus
*/
int8_t PIOS_Flash_W25X_EraseChip()
{
uint8_t ret;
uint8_t out[] = {W25X_CHIP_ERASE};
if((ret = PIOS_Flash_W25X_WriteEnable()) != 0)
return ret;
if(PIOS_Flash_W25X_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
while(PIOS_Flash_W25X_Busy()) {
//TODO: Fail on timeout
#if defined(PIOS_INCLUDE_FREERTOS)
vTaskDelay(1);
#endif
}
return 0;
}
/**
* @brief Return number of entries in the fifo
*/
int32_t PIOS_ADXL345_Test()
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;
if(PIOS_ADXL345_ClaimBus() != 0)
return -2;
uint8_t buf[2] = {0,0};
uint8_t rec[2] = {0,0};
buf[0] = ADXL_WHOAMI | ADXL_READ_BIT;
if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL) < 0) {
PIOS_ADXL345_ReleaseBus();
return -3;
}
PIOS_ADXL345_ReleaseBus();
return (rec[1] == ADXL_DEVICE_ID) ? 0 : -4;
}
/**
* @brief Return number of entries in the fifo
*/
int32_t PIOS_ADXL345_FifoElements()
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;
if(PIOS_ADXL345_ClaimBus() != 0)
return -2;
uint8_t buf[2] = {0,0};
uint8_t rec[2] = {0,0};
buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT ; // Read fifo status
if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],sizeof(buf),NULL) < 0) {
PIOS_ADXL345_ReleaseBus();
return -3;
}
PIOS_ADXL345_ReleaseBus();
return rec[1] & 0x3f;
}
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;
}
bool PIOS_BMA180_IRQHandler(void)
{
bma180_irqs++;
const static uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
uint8_t pios_bma180_dmabuf[8];
// If we can't get the bus then just move on for efficiency
bool woken = false;
if(PIOS_BMA180_ClaimBusISR(&woken) != 0) {
return woken; // Something else is using bus, miss this data
}
PIOS_SPI_TransferBlock(dev->spi_id,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf,
sizeof(pios_bma180_dmabuf), NULL);
// TODO: Make this conversion depend on configuration scale
struct pios_bma180_data data;
// Don't release bus till data has copied
PIOS_BMA180_ReleaseBus(&woken);
// Must not return before releasing bus
if(fifoBuf_getFree(&dev->fifo) < sizeof(data))
return woken;
// Bottom two bits indicate new data and are constant zeros. Don't right
// shift because it drops sign bit
data.x = ((pios_bma180_dmabuf[2] << 8) | pios_bma180_dmabuf[1]);
data.y = ((pios_bma180_dmabuf[4] << 8) | pios_bma180_dmabuf[3]);
data.z = ((pios_bma180_dmabuf[6] << 8) | pios_bma180_dmabuf[5]);
data.x /= 4;
data.y /= 4;
data.z /= 4;
data.temperature = pios_bma180_dmabuf[7];
fifoBuf_putData(&dev->fifo, (uint8_t *) &data, sizeof(data));
return woken;
}
/**
* @brief Erase a sector on the flash chip
* @param[in] add Address of flash to erase
* @returns 0 if successful
* @retval -1 if unable to claim bus
* @retval
*/
int8_t PIOS_Flash_W25X_EraseSector(uint32_t addr)
{
uint8_t ret;
uint8_t out[] = {W25X_SECTOR_ERASE, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff};
if((ret = PIOS_Flash_W25X_WriteEnable()) != 0)
return ret;
if(PIOS_Flash_W25X_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
while(PIOS_Flash_W25X_Busy()) {
//TODO: Fail on timeout
#if defined(PIOS_INCLUDE_FREERTOS)
vTaskDelay(1);
#endif
}
return 0;
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \returns 0 if succesful
*/
int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
{
// THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION
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) {
return -1;
}
if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
return -2;
}
PIOS_MPU6000_ReleaseBus();
data->gyro_x = rec[1] << 8 | rec[2];
data->gyro_y = rec[3] << 8 | rec[4];
data->gyro_z = rec[5] << 8 | rec[6];
return 0;
}
/**
* @brief Read a single set of values from the x y z channels
* @param[out] data Int16 array of (x,y,z) sensor values
* @returns 0 if successful
* @retval -1 unable to claim bus
* @retval -2 unable to transfer data
*/
int32_t PIOS_BMA180_ReadAccels(struct pios_bma180_data * data)
{
// To save memory use same buffer for in and out but offset by
// a byte
uint8_t buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
uint8_t rec[7] = {0,0,0,0,0,0};
if(PIOS_BMA180_ClaimBus() != 0)
return -1;
if(PIOS_SPI_TransferBlock(dev->spi_id,&buf[0],&rec[0],7,NULL) != 0)
return -2;
PIOS_BMA180_ReleaseBus();
// | MSB | LSB | 0 | new_data |
data->x = ((rec[2] << 8) | rec[1]);
data->y = ((rec[4] << 8) | rec[3]);
data->z = ((rec[6] << 8) | rec[5]);
data->x /= 4;
data->y /= 4;
data->z /= 4;
return 0; // return number of remaining entries
}
