/**
* @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;
}
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);
}
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);
}
/**
* @brief Release the SPI bus sempahore and ensure flash chip not using bus
*/
static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev *flash_dev)
{
PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 1);
PIOS_SPI_ReleaseBus(flash_dev->spi_id);
flash_dev->claimed = false;
return 0;
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* @return 0 if successful
* @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_ReleaseBusISR(bool *woken)
{
if (PIOS_MPU6000_Validate(dev) != 0) {
return -1;
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
}
/**
* @brief Release the SPI bus sempahore and ensure flash chip not using bus
*/
static int32_t PIOS_Flash_Jedec_ReleaseBus()
{
if(PIOS_Flash_Jedec_Validate(flash_dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 1);
PIOS_SPI_ReleaseBus(flash_dev->spi_id);
flash_dev->claimed = false;
return 0;
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* \param[in] pointer which receives if a task has been woken
* @return 0 if successful, -1 for invalid device
*/
static int32_t PIOS_L3GD20_ReleaseBusIsr(bool *woken)
{
if (PIOS_L3GD20_Validate(pios_l3gd20_dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(pios_l3gd20_dev->spi_id, pios_l3gd20_dev->slave_num, 1);
return PIOS_SPI_ReleaseBusISR(pios_l3gd20_dev->spi_id, woken);
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* \param[in] pointer which receives if a task has been woken
* @return 0 if successful
*/
int32_t PIOS_BMA180_ReleaseBusISR(bool *woken)
{
if(PIOS_BMA180_Validate(dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1);
return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* @return 0 if successful
*/
static int32_t PIOS_MPU6000_ReleaseBus()
{
if (PIOS_MPU6000_Validate(pios_mpu6000_dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(pios_mpu6000_dev->spi_id, pios_mpu6000_dev->slave_num, 1);
return PIOS_SPI_ReleaseBus(pios_mpu6000_dev->spi_id);
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* @return 0 if successful
*/
int32_t PIOS_BMA180_ReleaseBus()
{
if(PIOS_BMA180_Validate(dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,1);
return PIOS_SPI_ReleaseBus(dev->spi_id);
}
/**
* @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)
{
if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0)
return -1;
PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0);
flash_dev->claimed = true;
return 0;
}
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 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
*/
static int32_t PIOS_MPU6000_ClaimBus()
{
if (PIOS_MPU6000_Validate(pios_mpu6000_dev) != 0)
return -1;
if (PIOS_SPI_ClaimBus(pios_mpu6000_dev->spi_id) != 0)
return -2;
PIOS_SPI_RC_PinSet(pios_mpu6000_dev->spi_id, pios_mpu6000_dev->slave_num, 0);
return 0;
}
/**
* @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
*/
static int32_t PIOS_L3GD20_ClaimBus()
{
if(PIOS_L3GD20_Validate(dev) != 0)
return -1;
if(PIOS_SPI_ClaimBus(dev->spi_id) != 0)
return -2;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
return 0;
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* \param[in] pointer which receives if a task has been woken
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/
static int32_t PIOS_L3GD20_ClaimBusIsr(bool *woken)
{
if(PIOS_L3GD20_Validate(dev) != 0)
return -1;
if(PIOS_SPI_ClaimBusISR(dev->spi_id, woken) < 0)
return -2;
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
return 0;
}
/**
* @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
* @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_ClaimBusISR(bool *woken)
{
if (PIOS_MPU6000_Validate(dev) != 0) {
return -1;
}
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -2;
}
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
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 accel communications and end the transaction
* @return 0 if success or <0 for failure
*/
static int32_t PIOS_ADXL345_ReleaseBus()
{
if(PIOS_ADXL345_Validate(dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
if(PIOS_SPI_ReleaseBus(dev->spi_id) != 0)
return -2;
return 0;
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* \param[in] pointer which receives if a task has been woken
* @return 0 if successful, -1 if unable to claim bus
*/
int32_t PIOS_BMA180_ClaimBusISR(bool *woken)
{
if(PIOS_BMA180_Validate(dev) != 0)
return -1;
if(PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -1;
}
PIOS_SPI_RC_PinSet(dev->spi_id,dev->slave_num,0);
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 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()
{
if(PIOS_Flash_Jedec_Validate(flash_dev) != 0)
return -1;
if(PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0)
return -1;
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;
}
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 SendPacket(void)
{
PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0);
//no point checking if this failed. There isn't much we could do about it if it did fail
PIOS_SPI_TransferBlock(PIOS_OPAHRS_SPI, (uint8_t *) & txPacket, (uint8_t *) & rxPacket, sizeof(CommsDataPacket), &CommsCallback);
}
