void SPI_WriteData(uint8_t address, uint16_t data){
//uint16_t status;
uint16_t ui16Address;
ui16Address = address;
ui16Address <<= 3;
ui16Address |= 0x04; // Set RW bit
ui16Address += CalcParity(ui16Address); // Add parity bit
// Add parity to data
data <<= 1;
data += CalcParity(data);
GPIO_ResetBits(GPIOB, GPIO_Pin_7); // Select gyroscope
while(!SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE));
SPI_I2S_SendData(SPI1, address);
while(!SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE));
SPI_I2S_ReceiveData(SPI1);
while(!SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE));
SPI_I2S_SendData(SPI1, data);
while(!SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE));
SPI_I2S_ReceiveData(SPI1);
GPIO_SetBits(GPIOB, GPIO_Pin_7); // Deselect gyrosope
//return status;
}
uint16_t SPI_ReadData(uint8_t address){
uint16_t convertedGyro;
uint16_t ui16Address;
// Build address transfer frame
ui16Address = address;
ui16Address <<= 3; // Address to be shifted left by 3
ui16Address += CalcParity(ui16Address); //Add parity
GPIO_ResetBits(GPIOB, GPIO_Pin_7); // Select gyroscope
ui16Address = 0x80 | ui16Address; //0x80 = 10000000. Setting the MSB high tells the sensor to read and not to write.
//status = SPI1->DR;
SPI_I2S_ReceiveData(SPI1); // Read RX buffer just to clear interrupt flag.
while(!SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE));
SPI_I2S_SendData(SPI1, ui16Address);
while(!SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE));
SPI_I2S_ReceiveData(SPI1); //Clear RXNE bit
while(!SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE));
SPI_I2S_SendData(SPI1, 0x01); //Dummy byte (with correct parity) to generate clock
while(!SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE));
GPIO_SetBits(GPIOB, GPIO_Pin_7); // Deselect gyroscope
//Convert data from gyro rate register to angular speed (degrees per second):
convertedGyro = SPI_I2S_ReceiveData(SPI1);
convertedGyro >>= 2; //Bits 1:0 do not contain angular speed data
convertedGyro /= 18; //The sensor's sensitivity is 18 LSB/dps
return convertedGyro;
}
uint16_t ADXRS450_Gyro::ReadRegister(uint8_t reg) {
uint32_t cmd = 0x80000000 | (((uint32_t)reg) << 17);
if (!CalcParity(cmd)) cmd |= 1u;
// big endian
uint8_t buf[4] = {(uint8_t)((cmd >> 24) & 0xff),
(uint8_t)((cmd >> 16) & 0xff),
(uint8_t)((cmd >> 8) & 0xff),
(uint8_t)(cmd & 0xff)};
m_spi.Write(buf, 4);
m_spi.Read(false, buf, 4);
if ((buf[0] & 0xe0) == 0) return 0; // error, return 0
return (uint16_t)((BytesToIntBE(buf) >> 5) & 0xffff);
}
/**
* Reset the gyro.
* Resets the gyro to a heading of zero. This can be used if there is
* significant
* drift in the gyro and it needs to be recalibrated after it has been running.
*/
void ADXRS450_Gyro::Reset() {
m_spi.ResetAccumulator();
}
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by the
* oversampling rate, the
* gyro type and the A/D calibration values.
* The angle is continuous, that is it will continue from 360->361 degrees. This
* allows algorithms that wouldn't
* want to see a discontinuity in the gyro output as it sweeps from 360 to 0 on
* the second time around.
*
* @return the current heading of the robot in degrees. This heading is based on
* integration
* of the returned rate from the gyro.
*/
float ADXRS450_Gyro::GetAngle() const {
return (float)(m_spi.GetAccumulatorValue() * kDegreePerSecondPerLSB *
kSamplePeriod);
}
/**
* Return the rate of rotation of the gyro
*
* The rate is based on the most recent reading of the gyro analog value
*
* @return the current rate in degrees per second
*/
double ADXRS450_Gyro::GetRate() const {
return (double)m_spi.GetAccumulatorLastValue() * kDegreePerSecondPerLSB;
}
