/** Write a Gyroscope Register.
* I2C should aready be initialized!
*
* \param reg Register Address
* \param val New Value
* \returns #TWI_NO_ANSWER, #TWI_WRITE_ERROR or #SUCCESS.
*/
Error gyroWriteByte(uint8_t reg, uint8_t val) {
if (twiStart(GYRO_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(reg)) {
return TWI_WRITE_ERROR;
}
if (twiWrite(val)) {
return TWI_WRITE_ERROR;
}
twiStop();
return SUCCESS;
}
/** Write a Magnetometer Register.
* I2C should aready be initialized!
*
* \param reg Register Address
* \param val New Value
* \returns #TWI_NO_ANSWER, #TWI_WRITE_ERROR or #SUCCESS.
*/
Error magWriteRegister(uint8_t reg, uint8_t val) {
if (twiStart(MAG_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(reg)) {
return TWI_WRITE_ERROR;
}
if (twiWrite(val)) {
return TWI_WRITE_ERROR;
}
twiStop();
return SUCCESS;
}
// Set initial accelerometer parameters....modify as appropriate for your design.
void GadgetShield::accel_init(void)
{
twiWrite("\x07\x00", 2); // Access mode register, place device in standby mode so we can modify other regs
twiWrite(
"\x05" // Set address of sleep count register
"\x00" // Reset sleep count register to 0
"\xE0" // Enable shake interrupts (X/Y/Z)
"\x00" // Write 0 to mode register again, just so we don't start another write cycle
"\x00" // Reset all filtering, auto-wake averaging, and set 120 samples/s
"\x00" // Reset tap-pulse detection register to threshold of 1, all axes enabled for tap
"\x60" // Set tap-pulse debounce count register to 96 (seems to work well)
, 7);
twiWrite("\x07\x41", 2); // Enter active mode, set interrupt output to push-pull
}
void lcdPutString(char* data) {
twiStartWait(LCD_ADDRESS+I2C_WRITE);
while(*data != '\0') {
lcdCharsSent++;
if (*data != '\r') {
if (*data == '\n') {
twiWrite('\r');
twiWrite('\n');
data++;
} else {
twiWrite(*data++);
}
} else {
data++;
}
}
twiStop();
if (lcdCharsSent >= CHARSTOSEND) {
lcdCharsSent = 0;
_delay_ms(TIMETOWAIT);
}
}
Error gyroRead(Vector3f *v) {
// Simple Software Low-Pass
static double gyroSumX = 0, gyroSumY = 0, gyroSumZ = 0;
static double gyroFilterX = 0, gyroFilterY = 0, gyroFilterZ = 0;
if (v == NULL) {
return ARGUMENT_ERROR;
}
if (twiStart(GYRO_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(GYROREG_OUTXL | 0x80)) { // Auto Increment
return TWI_WRITE_ERROR;
}
if (twiRepStart(GYRO_ADDRESS | TWI_READ)) {
return TWI_NO_ANSWER;
}
uint8_t xl = twiReadAck();
uint8_t xh = twiReadAck();
uint8_t yl = twiReadAck();
uint8_t yh = twiReadAck();
uint8_t zl = twiReadAck();
uint8_t zh = twiReadNak();
int16_t x = *(int8_t *)(&xh);
x *= (1 << 8);
x |= xl;
int16_t y = *(int8_t *)(&yh);
y *= (1 << 8);
y |= yl;
int16_t z = *(int8_t *)(&zh);
z *= (1 << 8);
z |= zl;
switch (gyroRange) {
case r250DPS:
v->x = (((double)x) * 250 / 0x8000);
v->y = (((double)y) * 250 / 0x8000);
v->z = (((double)z) * 250 / 0x8000);
break;
case r500DPS:
v->x = (((double)x) * 500 / 0x8000);
v->y = (((double)y) * 500 / 0x8000);
v->z = (((double)z) * 500 / 0x8000);
break;
case r2000DPS:
v->x = (((double)x) * 2000 / 0x8000);
v->y = (((double)y) * 2000 / 0x8000);
v->z = (((double)z) * 2000 / 0x8000);
break;
default:
return ARGUMENT_ERROR;
}
gyroSumX = gyroSumX - gyroFilterX + v->x;
gyroFilterX = gyroSumX / GYROFILTERFACTOR;
v->x = gyroFilterX;
gyroSumY = gyroSumY - gyroFilterY + v->y;
gyroFilterY = gyroSumY / GYROFILTERFACTOR;
v->y = gyroFilterY;
gyroSumZ = gyroSumZ - gyroFilterZ + v->z;
gyroFilterZ = gyroSumZ / GYROFILTERFACTOR;
v->z = gyroFilterZ;
return SUCCESS;
}
Error magRead(Vector3f *v) {
if (v == NULL) {
return ARGUMENT_ERROR;
}
if (twiStart(MAG_ADDRESS | TWI_WRITE)) {
return TWI_NO_ANSWER;
}
if (twiWrite(MAGREG_XH)) {
return TWI_WRITE_ERROR;
}
if (twiRepStart(MAG_ADDRESS | TWI_READ)) {
return TWI_NO_ANSWER;
}
uint8_t xh = twiReadAck();
uint8_t xl = twiReadAck();
uint8_t zh = twiReadAck();
uint8_t zl = twiReadAck();
uint8_t yh = twiReadAck();
uint8_t yl = twiReadNak();
int16_t x = *(int8_t *)(&xh);
x *= (1 << 8);
x |= xl;
int16_t y = *(int8_t *)(&yh);
y *= (1 << 8);
y |= yl;
int16_t z = *(int8_t *)(&zh);
z *= (1 << 8);
z |= zl;
switch (magRange) {
case r1g3:
v->x = (((double)x) * 1.3 / MAG_NORMALIZE);
v->y = (((double)y) * 1.3 / MAG_NORMALIZE);
v->z = (((double)z) * 1.3 / MAG_NORMALIZE);
break;
case r1g9:
v->x = (((double)x) * 1.9 / MAG_NORMALIZE);
v->y = (((double)y) * 1.9 / MAG_NORMALIZE);
v->z = (((double)z) * 1.9 / MAG_NORMALIZE);
break;
case r2g5:
v->x = (((double)x) * 2.5 / MAG_NORMALIZE);
v->y = (((double)y) * 2.5 / MAG_NORMALIZE);
v->z = (((double)z) * 2.5 / MAG_NORMALIZE);
break;
case r4g0:
v->x = (((double)x) * 4.0 / MAG_NORMALIZE);
v->y = (((double)y) * 4.0 / MAG_NORMALIZE);
v->z = (((double)z) * 4.0 / MAG_NORMALIZE);
break;
case r4g7:
v->x = (((double)x) * 4.7 / MAG_NORMALIZE);
v->y = (((double)y) * 4.7 / MAG_NORMALIZE);
v->z = (((double)z) * 4.7 / MAG_NORMALIZE);
break;
case r5g6:
v->x = (((double)x) * 5.6 / MAG_NORMALIZE);
v->y = (((double)y) * 5.6 / MAG_NORMALIZE);
v->z = (((double)z) * 5.6 / MAG_NORMALIZE);
break;
case r8g1:
v->x = (((double)x) * 8.1 / MAG_NORMALIZE);
v->y = (((double)y) * 8.1 / MAG_NORMALIZE);
v->z = (((double)z) * 8.1 / MAG_NORMALIZE);
break;
default:
return ARGUMENT_ERROR;
}
return SUCCESS;
}
void lcdPutChar(char c) {
twiStartWait(LCD_ADDRESS+I2C_WRITE);
twiWrite(c);
twiStop();
}