static int doCardCmd(uint8_t cmd, uint32_t arg)
{
int result;
int tries = 1000;
do {
result = SPIxfer(0xff);
if (tries-- <= 0) return -1;
} while(result != 0xff);
uint8_t crc = 0;
SPIxfer(crc7_update(&crc, cmd | 0x40));
SPIxfer(crc7_update(&crc, arg >> 24));
SPIxfer(crc7_update(&crc, arg >> 16));
SPIxfer(crc7_update(&crc, arg >> 8));
SPIxfer(crc7_update(&crc, arg));
SPIxfer(crc | 1);
do {
result = SPIxfer(0xff);
if (tries-- <= 0) return -1;
} while (result & 0x80);
return result;
}
// Reads a gyro register
byte L3G::readReg(byte reg)
{
byte value;
////begin SPI
//SPI.beginTransaction(SPISettings(SPI_CLOCK_DIV8, MSBFIRST, SPI_MODE0));
//digitalWrite(_cs, LOW);
////transfer
//SPI.transfer((uint8_t)reg | 0x80); // set READ bit
//value = SPI.transfer(0xFF);
////end SPI
//digitalWrite(_cs, HIGH);
//SPI.endTransaction();
digitalWrite(_clk, HIGH);
digitalWrite(_cs, LOW);
SPIxfer((uint8_t)reg | 0x80); // set READ bit
value = SPIxfer(0xFF);
digitalWrite(_cs, HIGH);
return value;
}
static uint16_t SPIread16(void)
{
uint16_t r = 0;
r |= SPIxfer(0xff) << 8;
r |= SPIxfer(0xff) << 0;
return r;
}
static uint32_t SPIread32(void)
{
uint32_t r = 0;
r |= SPIxfer(0xff) << 24;
r |= SPIxfer(0xff) << 16;
r |= SPIxfer(0xff) << 8;
r |= SPIxfer(0xff) << 0;
return r;
}
/***************************************************************************
PRIVATE FUNCTIONS
***************************************************************************/
void Adafruit_L3GD20::write8(l3gd20Registers_t reg, byte value)
{
if (_cs == -1) {
// use i2c
Wire.beginTransmission(address);
Wire.write((byte)reg);
Wire.write(value);
Wire.endTransmission();
} else {
digitalWrite(_clk, HIGH);
digitalWrite(_cs, LOW);
SPIxfer(reg);
SPIxfer(value);
digitalWrite(_cs, HIGH);
}
}
uint16_t SPIreaddata(int len)
{
int result;
int tries = 1000;
do {
result = SPIxfer(0xff);
if (tries-- <= 0) return -1;
} while (result != 0xfe);
uint16_t crc = 0;
for (int i = 0; i < len; i++) {
uint8_t dat = sdbuf[i] = SPIxfer(0xff);
crc16_update(crc, dat);
}
uint16_t recv_crc = SPIread16();
printf("Calculated CRC %04x, received CRC %04x\n", crc, recv_crc);
return crc != recv_crc;
}
// Writes a gyro register
void L3G::writeReg(byte reg, byte value)
{
////begin SPI
//SPI.beginTransaction(SPISettings(SPI_CLOCK_DIV8, MSBFIRST, SPI_MODE3));
//digitalWrite(_cs, LOW);
////transfer
//SPI.transfer(reg); // set READ bit
//SPI.transfer(value);
////end SPI
//digitalWrite(_cs, HIGH);
//SPI.endTransaction();
digitalWrite(_clk, HIGH);
digitalWrite(_cs, LOW);
SPIxfer(reg);
SPIxfer(value);
digitalWrite(_cs, HIGH);
}
// Reads the z gyro channels and stores it in z. Return true when new data.
bool L3G::read()
{
byte num_data = readReg(FIFO_SRC) & B00011111;
if(num_data > 0) //read only if we have data
{
long raw_z = 0;
//begin SPI
/*SPI.beginTransaction(SPISettings(SPI_CLOCK_DIV8, MSBFIRST, SPI_MODE3));
digitalWrite(_cs, LOW);*/
for(byte datum = 0; datum < num_data; ++datum)
{
digitalWrite(_cs, LOW);
/*SPI.transfer(OUT_Z_L | 0x80 | 0x40);
uint8_t zlg = SPI.transfer(0xFF);
uint8_t zhg = SPI.transfer(0xFF);*/
SPIxfer(OUT_Z_L | 0x80 | 0x40); // SPI read, autoincrement
uint8_t zlg = SPIxfer(0xFF);
uint8_t zhg = SPIxfer(0xFF);
digitalWrite(_cs, HIGH);
raw_z += ((int16_t)(zhg << 8 | zlg));
}
//end SPI
/*digitalWrite(_cs, HIGH);
SPI.endTransaction();*/
//Convert to DPS and average over number of data points we read. Negative since Z axis upside down.
z = -(raw_z * 0.00875f / num_data);
fresh_data = false;
return true;
}
return false;
}
byte Adafruit_L3GD20::read8(l3gd20Registers_t reg)
{
byte value;
if (_cs == -1) {
// use i2c
Wire.beginTransmission(address);
Wire.write((byte)reg);
Wire.endTransmission();
Wire.requestFrom(address, (byte)1);
value = Wire.read();
Wire.endTransmission();
} else {
digitalWrite(_clk, HIGH);
digitalWrite(_cs, LOW);
SPIxfer((uint8_t)reg | 0x80); // set READ bit
value = SPIxfer(0xFF);
digitalWrite(_cs, HIGH);
}
return value;
}
void sd_card_init(void)
{
int result;
sd_card_setspeed(400000);
// Power-on card - send 74+ (80) clocks to initialize card
GPIOPinWrite(GPIO_PORTD_BASE, SD_PIN_CS, SD_PIN_CS);
for (int i = 0; i < 10; i++)
SPIxfer(0xff);
SysCtlDelay(200);
GPIOPinWrite(GPIO_PORTD_BASE, SD_PIN_CS, 0);
SysCtlDelay(200);
result = doCardCmd(SD_CMD_GO_IDLE_STATE, 0);
printf("GO_IDLE_STATE -> %02x\n", result);
if (result == -1) return;
result = doCardCmd(SD_CMD_SEND_IF_COND, 0x1A5);
printf("SEND_IF_COND(1A5) = %02x\n", result);
if (result == -1) return;
uint32_t recv_cond = SPIread32();
printf("Condition: %02x%08x\n", result, recv_cond);
if ((recv_cond & 0x1A5) != 0x1A5) return;
printf("Waiting for card init");
int init_tries = 1500; // I've frequently seen 800
do {
putchar('.');
result = doCardAcmd(SD_ACMD_SD_SEND_OP_COND, 1UL << 30);
if (init_tries-- <= 0) {
printf(" card is not initializing.\n");
return;
}
} while (result == 0x01);
if (result != 0x00) {
printf("Error: %02x\n", result);
return;
} else
printf(" OK\n");
result = doCardCmd(SD_CMD_READ_OCR, 0);
uint32_t card_ocr = SPIread32();
printf("OCR = %08x\n", card_ocr);
/*
int init_tries = 2500; // I've frequently seen ~800!
printf("Waiting for card init.");
do {
printf(".");
result = doCardCmd(1, 0);
if (init_tries-- <= 0) {
printf(" card is not initializing.\n");
return;
}
} while (result == 0x01);
if (result != 0x00) {
printf("Error: %02x\n", result);
return;
} else
printf(" OK\n");
result = doCardCmd(16, 512);
printf("CMD16(512) => %x\n", result);
result = doCardCmd(17, 0);
printf("CMD17(0) => %x\n", result);
if (result == 0) {
uint16_t crc;
if (SPIreaddata(512) == 0) {
for (int i = 0; i < 512; i++) {
printf("%02x%c", sdbuf[i], (i & 15) == 15 ? '\n' : ' ');
}
}
}
*/
}
/***************************************************************************
PUBLIC FUNCTIONS
***************************************************************************/
void Adafruit_L3GD20::read()
{
uint8_t xhi, xlo, ylo, yhi, zlo, zhi;
if (_cs == -1) {
Wire.beginTransmission(address);
// Make sure to set address auto-increment bit
Wire.write(L3GD20_REGISTER_OUT_X_L | 0x80);
Wire.endTransmission();
Wire.requestFrom(address, (byte)6);
// Wait around until enough data is available
while (Wire.available() < 6);
xlo = Wire.read();
xhi = Wire.read();
ylo = Wire.read();
yhi = Wire.read();
zlo = Wire.read();
zhi = Wire.read();
} else {
digitalWrite(_clk, HIGH);
digitalWrite(_cs, LOW);
SPIxfer(L3GD20_REGISTER_OUT_X_L | 0x80 | 0x40); // SPI read, autoincrement
delay(10);
xlo = SPIxfer(0xFF);
xhi = SPIxfer(0xFF);
ylo = SPIxfer(0xFF);
yhi = SPIxfer(0xFF);
zlo = SPIxfer(0xFF);
zhi = SPIxfer(0xFF);
digitalWrite(_cs, HIGH);
}
// Shift values to create properly formed integer (low byte first)
data.x = (int16_t)(xlo | (xhi << 8));
data.y = (int16_t)(ylo | (yhi << 8));
data.z = (int16_t)(zlo | (zhi << 8));
// Compensate values depending on the resolution
switch(range)
{
case L3DS20_RANGE_250DPS:
data.x *= L3GD20_SENSITIVITY_250DPS;
data.y *= L3GD20_SENSITIVITY_250DPS;
data.z *= L3GD20_SENSITIVITY_250DPS;
break;
case L3DS20_RANGE_500DPS:
data.x *= L3GD20_SENSITIVITY_500DPS;
data.y *= L3GD20_SENSITIVITY_500DPS;
data.z *= L3GD20_SENSITIVITY_500DPS;
break;
case L3DS20_RANGE_2000DPS:
data.x *= L3GD20_SENSITIVITY_2000DPS;
data.y *= L3GD20_SENSITIVITY_2000DPS;
data.z *= L3GD20_SENSITIVITY_2000DPS;
break;
}
}
