/*static void SPI1_Read_REV(int16_t *acc_tmp);
{
uint8_t tmp[6];
uint8_t i;
MPU_ON;
spi_writeByte(MPU_RA_XA_OFFS_H | 0x80);
for (i = 0; i < 6; i++)
buf[i] = spi_readByte();
MPU_OFF;
acc_tmp[0]=tmp[0];
acc_tmp[1]=tmp[1];
acc_tmp[2]=tmp[2];
acc_tmp[3]=tmp[3];
acc_tmp[4]=tmp[4];
acc_tmp[5]=tmp[5];
}
*/
static void SPI1_Write(uint8_t Address, uint8_t Data )
{
MPU_ON;
spi_writeByte(Address);
spi_writeByte(Data);
MPU_OFF;
delay(1);
}
uint8_t sdmmc_writeCommand(uint8_t pCommand, uint32_t pArgument, uint8_t pCrc) {
// The command is composed out of 6 Byte:
// Byte 1: 0b01xxxxxx where xx = pCommand
// Byte 2-5: pArgument
// Byte 6: 0byyyyyyy1 where yy = CRC7 (ignored when card is in SPI mode)
// Make sure that bits 7 and 6 of pCommand are 0 and 1
pCommand &= ~(1 << 7);
pCommand |= (1 << 6);
// Calculate CRC7 (// TODO //, currently precalculated values)
// Bit 0 of the checksum byte has to be set
pCrc |= 1;
// Send some dummy clock signals
CLEAR_CS();
spi_writeByte(0xFF);
SET_CS();
// Send the data
spi_writeByte(pCommand);
spi_writeByte((uint8_t)((pArgument & 0xFF000000) >> 24));
spi_writeByte((uint8_t)((pArgument & 0x00FF0000) >> 16));
spi_writeByte((uint8_t)((pArgument & 0x0000FF00) >> 8));
spi_writeByte((uint8_t)(pArgument & 0x000000FF));
spi_writeByte(pCrc);
// Get a response
uint8_t result = 0xFF;
uint8_t retry = 0;
while(result == 0xFF) {
result = spi_readByte();
if (retry++ == 0xFF) {
result = 0;
break;
}
}
return result;
}
uint8_t sdmmc_writeSector(uint32_t pSectorNum, char* pInput) {
SET_CS();
// Send command 24 (WRITE_BLOCK)
if (sdmmc_writeCommand(SDMMC_WRITE_BLOCK, SECTOR_TO_BYTE(pSectorNum), SDMMC_DEFAULT_CRC) != 0) {
CLEAR_CS();
return FALSE;
}
// Send some dummy clocks
for(uint8_t i = 0; i < 100; i++) {
spi_readByte();
}
// Send start-byte
spi_writeByte(0xFE);
// Send data
for(uint16_t i = 0; i < fBlockLength; i++) {
spi_writeByte(pInput[i]);
}
// Send dummy CRC checksum (won't be checked in SPI mode)
spi_writeByte(0xFF);
spi_writeByte(0xFF);
// Get response
if ((spi_readByte() & 0x1F) != 0x05) {
CLEAR_CS();
return FALSE;
}
// Wait until memory card is ready for new commands
while (spi_readByte() != 0xFF) {
// burn energy
}
CLEAR_CS();
return TRUE;
}
static void mpu6000AccRead(int16_t *accData)
{
//uint8_t buf[6];
//SPI1_Read_Buf( MPU_RA_ACCEL_XOUT_H, 6, buf);
uint8_t buf[6];
uint8_t i;
MPU_ON;
spi_writeByte(MPU_RA_ACCEL_XOUT_H | 0x80);
for (i = 0; i < 6; i++)
buf[i] = spi_readByte();
MPU_OFF;
accData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 8;
accData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 8;
accData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 8;
}
static void mpu6000GyroRead(int16_t *gyroData)
{
uint8_t buf[6];
uint8_t i;
MPU_ON;
spi_writeByte(MPU_RA_GYRO_XOUT_H | 0x80);
//SPI1_Read_Buf(MPU_RA_GYRO_XOUT_H, 6, buf);
for (i = 0; i < 6; i++)
buf[i] = spi_readByte();
MPU_OFF;
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
}
void sdmmc_init() {
// Initializes SPI interface first
if (!spi_init()) {
error(ERROR_SDMMC);
}
uint8_t response = 0;
uint8_t retry = 0;
// Send some dummy bytes before actual data
for(uint8_t i = 0; i < 20; i++) {
spi_writeByte(0xFF);
}
// Send command 0 (GO_IDLE_STATE, i.e. change from SD into SPI mode)
while (response != 1) {
response = sdmmc_writeCommand(SDMMC_GO_IDLE_STATE, 0, SDMMC_GO_IDLE_STATE_CRC); // 0x95 is a precalculated CRC checksum
// If the device does not respond correctly, return FALSE after
// having it tried several times
if (retry++ == 0xFF) {
CLEAR_CS();
error(ERROR_SDMMC);
}
}
// Send copmmand 1 (SEND_OP_COND, initialize card)
response = 0xFF;
retry = 0;
while (response != 0) {
response = sdmmc_writeCommand(SDMMC_SEND_OP_COND, 0, SDMMC_DEFAULT_CRC);
if (retry++ == 0xFF) {
CLEAR_CS();
error(ERROR_SDMMC);
}
}
CLEAR_CS();
// switch to higher SPI frequency once initialized
spi_highspeed();
}
bool mpu6000Detect(sensor_t * acc, sensor_t * gyro, uint16_t lpf, uint8_t *scale)
{
// bool ack;
uint8_t sig;//, rev
//uint8_t tmp[6];
delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
//sig = SPI1_Read( MPU_RA_WHO_AM_I);
MPU_ON;
spi_writeByte(MPU_RA_WHO_AM_I | 0x80); // Address with high bit set = Read operation
sig = spi_readByte();
MPU_OFF;
// So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
// The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0’s 7-bit I2C address.
// The least significant bit of the MPU-60X0’s I2C address is determined by the value of the AD0 pin. (we know that already).
// But here's the best part: The value of the AD0 pin is not reflected in this register.
if (sig !=0x68) //0x7e)
return false;
/*/determine product ID and accel revision
SPI1_Read(int16_t *acc_tmp);
rev = ((acc_tmp[5] & 0x01) << 2) | ((acc_tmp[3] & 0x01) << 1) | (acc_tmp[1] & 0x01);
if (rev) {
// Congrats, these parts are better.
if (rev == 1) {
mpuAccelHalf = 1;
} else if (rev == 2) {
mpuAccelHalf = 0;
} else {
failureMode(5);
}
} else {
sig = SPI1_Read( MPU_RA_PRODUCT_ID, 1, &sig);
rev = sig & 0x0F;
if (!rev) {
failureMode(5);
} else if (rev == 4) {
mpuAccelHalf = 1;
} else {
mpuAccelHalf = 0;
}
}*/
acc->init = mpu6000AccInit;
acc->read = mpu6000AccRead;
acc->align = mpu6000AccAlign;
gyro->init = mpu6000GyroInit;
gyro->read = mpu6000GyroRead;
gyro->align = mpu6000GyroAlign;
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
// give halfacc (old revision) back to system
if (scale)
*scale = mpuAccelHalf;
// default lpf is 42Hz
switch (lpf) {
case 256:
mpuLowPassFilter = MPU6000_LPF_256HZ;
break;
case 188:
mpuLowPassFilter = MPU6000_LPF_188HZ;
break;
case 98:
mpuLowPassFilter = MPU6000_LPF_98HZ;
break;
default:
case 42:
mpuLowPassFilter = MPU6000_LPF_42HZ;
break;
case 20:
mpuLowPassFilter = MPU6000_LPF_20HZ;
break;
case 10:
mpuLowPassFilter = MPU6000_LPF_10HZ;
break;
case 5:
mpuLowPassFilter = MPU6000_LPF_5HZ;
break;
}
return true;
}
